View Javadoc
1   /*
2    * Copyright (C) 2020 Alberto Irurueta Carro (alberto@irurueta.com)
3    *
4    * Licensed under the Apache License, Version 2.0 (the "License");
5    * you may not use this file except in compliance with the License.
6    * You may obtain a copy of the License at
7    *
8    *         http://www.apache.org/licenses/LICENSE-2.0
9    *
10   * Unless required by applicable law or agreed to in writing, software
11   * distributed under the License is distributed on an "AS IS" BASIS,
12   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13   * See the License for the specific language governing permissions and
14   * limitations under the License.
15   */
16  package com.irurueta.navigation.inertial.calibration.accelerometer;
17  
18  import com.irurueta.algebra.Matrix;
19  import com.irurueta.algebra.WrongSizeException;
20  import com.irurueta.navigation.LockedException;
21  import com.irurueta.navigation.NotReadyException;
22  import com.irurueta.navigation.frames.CoordinateTransformation;
23  import com.irurueta.navigation.frames.ECEFFrame;
24  import com.irurueta.navigation.frames.FrameType;
25  import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
26  import com.irurueta.navigation.frames.NEDFrame;
27  import com.irurueta.navigation.frames.converters.NEDtoECEFFrameConverter;
28  import com.irurueta.navigation.frames.converters.NEDtoECEFPositionVelocityConverter;
29  import com.irurueta.navigation.inertial.BodyKinematics;
30  import com.irurueta.navigation.inertial.ECEFGravity;
31  import com.irurueta.navigation.inertial.ECEFPosition;
32  import com.irurueta.navigation.inertial.ECEFVelocity;
33  import com.irurueta.navigation.inertial.NEDPosition;
34  import com.irurueta.navigation.inertial.NEDVelocity;
35  import com.irurueta.navigation.inertial.calibration.BodyKinematicsGenerator;
36  import com.irurueta.navigation.inertial.calibration.CalibrationException;
37  import com.irurueta.navigation.inertial.calibration.IMUErrors;
38  import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyKinematics;
39  import com.irurueta.navigation.inertial.estimators.ECEFGravityEstimator;
40  import com.irurueta.navigation.inertial.estimators.ECEFKinematicsEstimator;
41  import com.irurueta.statistics.UniformRandomizer;
42  import com.irurueta.units.Acceleration;
43  import com.irurueta.units.AccelerationUnit;
44  import org.junit.Test;
45  
46  import java.util.ArrayList;
47  import java.util.Collection;
48  import java.util.Collections;
49  import java.util.List;
50  import java.util.Random;
51  
52  import static org.junit.Assert.*;
53  
54  public class KnownBiasAndGravityNormAccelerometerCalibratorTest implements
55          KnownBiasAndGravityNormAccelerometerCalibratorListener {
56  
57      private static final double TIME_INTERVAL_SECONDS = 0.02;
58  
59      private static final double MICRO_G_TO_METERS_PER_SECOND_SQUARED = 9.80665E-6;
60      private static final double DEG_TO_RAD = 0.01745329252;
61  
62      private static final double MIN_ANGLE_DEGREES = -180.0;
63      private static final double MAX_ANGLE_DEGREES = 180.0;
64  
65      private static final double MIN_LATITUDE_DEGREES = -90.0;
66      private static final double MAX_LATITUDE_DEGREES = 90.0;
67      private static final double MIN_LONGITUDE_DEGREES = -180.0;
68      private static final double MAX_LONGITUDE_DEGREES = 180.0;
69      private static final double MIN_HEIGHT = -50.0;
70      private static final double MAX_HEIGHT = 50.0;
71  
72      private static final int LARGE_MEASUREMENT_NUMBER = 70000;
73  
74      private static final double ABSOLUTE_ERROR = 1e-8;
75      private static final double LARGE_ABSOLUTE_ERROR = 5e-5;
76  
77      private static final int TIMES = 70;
78  
79      private int mCalibrateStart;
80      private int mCalibrateEnd;
81  
82      @Test
83      public void testConstructor1() throws WrongSizeException {
84          final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
85                  new KnownBiasAndGravityNormAccelerometerCalibrator();
86  
87          // check default values
88          assertEquals(calibrator.getBiasX(), 0.0, 0.0);
89          assertEquals(calibrator.getBiasY(), 0.0, 0.0);
90          assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
91          final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
92          assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
93          assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
94          final Acceleration bx2 = new Acceleration(0.0,
95                  AccelerationUnit.FEET_PER_SQUARED_SECOND);
96          calibrator.getBiasXAsAcceleration(bx2);
97          assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
98          assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
99          final Acceleration by1 = calibrator.getBiasYAsAcceleration();
100         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
101         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
102         final Acceleration by2 = new Acceleration(0.0,
103                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
104         calibrator.getBiasYAsAcceleration(by2);
105         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
106         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
107         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
108         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
109         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
110         final Acceleration bz2 = new Acceleration(0.0,
111                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
112         calibrator.getBiasZAsAcceleration(bz2);
113         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
114         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
115         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
116         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
117         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
118         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
119         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
120         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
121         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
122         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
123         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
124         final double[] bias1 = calibrator.getBias();
125         assertArrayEquals(bias1, new double[3], 0.0);
126         final double[] bias2 = new double[3];
127         calibrator.getBias(bias2);
128         assertArrayEquals(bias1, bias2, 0.0);
129         final Matrix b1 = calibrator.getBiasAsMatrix();
130         assertEquals(b1, new Matrix(3, 1));
131         final Matrix b2 = new Matrix(3, 1);
132         calibrator.getBiasAsMatrix(b2);
133         assertEquals(b1, b2);
134         final Matrix ma1 = calibrator.getInitialMa();
135         assertEquals(ma1, new Matrix(3, 3));
136         final Matrix ma2 = new Matrix(3, 3);
137         calibrator.getInitialMa(ma2);
138         assertEquals(ma1, ma2);
139         assertNull(calibrator.getMeasurements());
140         assertFalse(calibrator.isCommonAxisUsed());
141         assertNull(calibrator.getListener());
142         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
143         assertFalse(calibrator.isReady());
144         assertFalse(calibrator.isRunning());
145         assertNull(calibrator.getEstimatedMa());
146         assertNull(calibrator.getEstimatedSx());
147         assertNull(calibrator.getEstimatedSy());
148         assertNull(calibrator.getEstimatedSz());
149         assertNull(calibrator.getEstimatedMxy());
150         assertNull(calibrator.getEstimatedMxz());
151         assertNull(calibrator.getEstimatedMyx());
152         assertNull(calibrator.getEstimatedMyz());
153         assertNull(calibrator.getEstimatedMzx());
154         assertNull(calibrator.getEstimatedMzy());
155         assertNull(calibrator.getEstimatedCovariance());
156         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
157         assertNull(calibrator.getGroundTruthGravityNorm());
158         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
159         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
160     }
161 
162     @Test
163     public void testConstructor2() throws WrongSizeException {
164         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
165                 new KnownBiasAndGravityNormAccelerometerCalibrator(this);
166 
167         // check default values
168         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
169         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
170         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
171         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
172         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
173         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
174         final Acceleration bx2 = new Acceleration(0.0,
175                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
176         calibrator.getBiasXAsAcceleration(bx2);
177         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
178         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
179         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
180         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
181         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
182         final Acceleration by2 = new Acceleration(0.0,
183                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
184         calibrator.getBiasYAsAcceleration(by2);
185         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
186         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
187         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
188         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
189         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
190         final Acceleration bz2 = new Acceleration(0.0,
191                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
192         calibrator.getBiasZAsAcceleration(bz2);
193         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
194         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
195         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
196         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
197         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
198         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
199         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
200         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
201         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
202         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
203         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
204         final double[] bias1 = calibrator.getBias();
205         assertArrayEquals(bias1, new double[3], 0.0);
206         final double[] bias2 = new double[3];
207         calibrator.getBias(bias2);
208         assertArrayEquals(bias1, bias2, 0.0);
209         final Matrix b1 = calibrator.getBiasAsMatrix();
210         assertEquals(b1, new Matrix(3, 1));
211         final Matrix b2 = new Matrix(3, 1);
212         calibrator.getBiasAsMatrix(b2);
213         assertEquals(b1, b2);
214         final Matrix ma1 = calibrator.getInitialMa();
215         assertEquals(ma1, new Matrix(3, 3));
216         final Matrix ma2 = new Matrix(3, 3);
217         calibrator.getInitialMa(ma2);
218         assertEquals(ma1, ma2);
219         assertNull(calibrator.getMeasurements());
220         assertFalse(calibrator.isCommonAxisUsed());
221         assertSame(calibrator.getListener(), this);
222         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
223         assertFalse(calibrator.isReady());
224         assertFalse(calibrator.isRunning());
225         assertNull(calibrator.getEstimatedMa());
226         assertNull(calibrator.getEstimatedSx());
227         assertNull(calibrator.getEstimatedSy());
228         assertNull(calibrator.getEstimatedSz());
229         assertNull(calibrator.getEstimatedMxy());
230         assertNull(calibrator.getEstimatedMxz());
231         assertNull(calibrator.getEstimatedMyx());
232         assertNull(calibrator.getEstimatedMyz());
233         assertNull(calibrator.getEstimatedMzx());
234         assertNull(calibrator.getEstimatedMzy());
235         assertNull(calibrator.getEstimatedCovariance());
236         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
237         assertNull(calibrator.getGroundTruthGravityNorm());
238         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
239         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
240     }
241 
242     @Test
243     public void testConstructor3() throws WrongSizeException {
244         final Collection<StandardDeviationBodyKinematics> measurements =
245                 Collections.emptyList();
246         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
247                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements);
248 
249         // check default values
250         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
251         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
252         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
253         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
254         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
255         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
256         final Acceleration bx2 = new Acceleration(0.0,
257                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
258         calibrator.getBiasXAsAcceleration(bx2);
259         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
260         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
261         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
262         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
263         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
264         final Acceleration by2 = new Acceleration(0.0,
265                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
266         calibrator.getBiasYAsAcceleration(by2);
267         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
268         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
269         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
270         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
271         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
272         final Acceleration bz2 = new Acceleration(0.0,
273                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
274         calibrator.getBiasZAsAcceleration(bz2);
275         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
276         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
277         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
278         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
279         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
280         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
281         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
282         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
283         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
284         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
285         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
286         final double[] bias1 = calibrator.getBias();
287         assertArrayEquals(bias1, new double[3], 0.0);
288         final double[] bias2 = new double[3];
289         calibrator.getBias(bias2);
290         assertArrayEquals(bias1, bias2, 0.0);
291         final Matrix b1 = calibrator.getBiasAsMatrix();
292         assertEquals(b1, new Matrix(3, 1));
293         final Matrix b2 = new Matrix(3, 1);
294         calibrator.getBiasAsMatrix(b2);
295         assertEquals(b1, b2);
296         final Matrix ma1 = calibrator.getInitialMa();
297         assertEquals(ma1, new Matrix(3, 3));
298         final Matrix ma2 = new Matrix(3, 3);
299         calibrator.getInitialMa(ma2);
300         assertEquals(ma1, ma2);
301         assertSame(calibrator.getMeasurements(), measurements);
302         assertFalse(calibrator.isCommonAxisUsed());
303         assertNull(calibrator.getListener());
304         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
305         assertFalse(calibrator.isReady());
306         assertFalse(calibrator.isRunning());
307         assertNull(calibrator.getEstimatedMa());
308         assertNull(calibrator.getEstimatedSx());
309         assertNull(calibrator.getEstimatedSy());
310         assertNull(calibrator.getEstimatedSz());
311         assertNull(calibrator.getEstimatedMxy());
312         assertNull(calibrator.getEstimatedMxz());
313         assertNull(calibrator.getEstimatedMyx());
314         assertNull(calibrator.getEstimatedMyz());
315         assertNull(calibrator.getEstimatedMzx());
316         assertNull(calibrator.getEstimatedMzy());
317         assertNull(calibrator.getEstimatedCovariance());
318         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
319         assertNull(calibrator.getGroundTruthGravityNorm());
320         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
321         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
322     }
323 
324     @Test
325     public void testConstructor4() throws WrongSizeException {
326         final Collection<StandardDeviationBodyKinematics> measurements =
327                 Collections.emptyList();
328         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
329                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
330                         this);
331 
332         // check default values
333         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
334         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
335         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
336         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
337         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
338         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
339         final Acceleration bx2 = new Acceleration(0.0,
340                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
341         calibrator.getBiasXAsAcceleration(bx2);
342         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
343         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
344         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
345         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
346         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
347         final Acceleration by2 = new Acceleration(0.0,
348                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
349         calibrator.getBiasYAsAcceleration(by2);
350         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
351         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
352         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
353         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
354         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
355         final Acceleration bz2 = new Acceleration(0.0,
356                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
357         calibrator.getBiasZAsAcceleration(bz2);
358         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
359         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
360         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
361         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
362         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
363         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
364         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
365         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
366         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
367         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
368         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
369         final double[] bias1 = calibrator.getBias();
370         assertArrayEquals(bias1, new double[3], 0.0);
371         final double[] bias2 = new double[3];
372         calibrator.getBias(bias2);
373         assertArrayEquals(bias1, bias2, 0.0);
374         final Matrix b1 = calibrator.getBiasAsMatrix();
375         assertEquals(b1, new Matrix(3, 1));
376         final Matrix b2 = new Matrix(3, 1);
377         calibrator.getBiasAsMatrix(b2);
378         assertEquals(b1, b2);
379         final Matrix ma1 = calibrator.getInitialMa();
380         assertEquals(ma1, new Matrix(3, 3));
381         final Matrix ma2 = new Matrix(3, 3);
382         calibrator.getInitialMa(ma2);
383         assertEquals(ma1, ma2);
384         assertSame(calibrator.getMeasurements(), measurements);
385         assertFalse(calibrator.isCommonAxisUsed());
386         assertSame(calibrator.getListener(), this);
387         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
388         assertFalse(calibrator.isReady());
389         assertFalse(calibrator.isRunning());
390         assertNull(calibrator.getEstimatedMa());
391         assertNull(calibrator.getEstimatedSx());
392         assertNull(calibrator.getEstimatedSy());
393         assertNull(calibrator.getEstimatedSz());
394         assertNull(calibrator.getEstimatedMxy());
395         assertNull(calibrator.getEstimatedMxz());
396         assertNull(calibrator.getEstimatedMyx());
397         assertNull(calibrator.getEstimatedMyz());
398         assertNull(calibrator.getEstimatedMzx());
399         assertNull(calibrator.getEstimatedMzy());
400         assertNull(calibrator.getEstimatedCovariance());
401         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
402         assertNull(calibrator.getGroundTruthGravityNorm());
403         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
404         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
405     }
406 
407     @Test
408     public void testConstructor5() throws WrongSizeException {
409         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
410                 new KnownBiasAndGravityNormAccelerometerCalibrator(true);
411 
412         // check default values
413         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
414         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
415         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
416         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
417         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
418         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
419         final Acceleration bx2 = new Acceleration(0.0,
420                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
421         calibrator.getBiasXAsAcceleration(bx2);
422         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
423         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
424         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
425         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
426         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
427         final Acceleration by2 = new Acceleration(0.0,
428                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
429         calibrator.getBiasYAsAcceleration(by2);
430         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
431         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
432         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
433         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
434         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
435         final Acceleration bz2 = new Acceleration(0.0,
436                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
437         calibrator.getBiasZAsAcceleration(bz2);
438         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
439         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
440         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
441         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
442         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
443         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
444         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
445         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
446         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
447         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
448         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
449         final double[] bias1 = calibrator.getBias();
450         assertArrayEquals(bias1, new double[3], 0.0);
451         final double[] bias2 = new double[3];
452         calibrator.getBias(bias2);
453         assertArrayEquals(bias1, bias2, 0.0);
454         final Matrix b1 = calibrator.getBiasAsMatrix();
455         assertEquals(b1, new Matrix(3, 1));
456         final Matrix b2 = new Matrix(3, 1);
457         calibrator.getBiasAsMatrix(b2);
458         assertEquals(b1, b2);
459         final Matrix ma1 = calibrator.getInitialMa();
460         assertEquals(ma1, new Matrix(3, 3));
461         final Matrix ma2 = new Matrix(3, 3);
462         calibrator.getInitialMa(ma2);
463         assertEquals(ma1, ma2);
464         assertNull(calibrator.getMeasurements());
465         assertTrue(calibrator.isCommonAxisUsed());
466         assertNull(calibrator.getListener());
467         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
468         assertFalse(calibrator.isReady());
469         assertFalse(calibrator.isRunning());
470         assertNull(calibrator.getEstimatedMa());
471         assertNull(calibrator.getEstimatedSx());
472         assertNull(calibrator.getEstimatedSy());
473         assertNull(calibrator.getEstimatedSz());
474         assertNull(calibrator.getEstimatedMxy());
475         assertNull(calibrator.getEstimatedMxz());
476         assertNull(calibrator.getEstimatedMyx());
477         assertNull(calibrator.getEstimatedMyz());
478         assertNull(calibrator.getEstimatedMzx());
479         assertNull(calibrator.getEstimatedMzy());
480         assertNull(calibrator.getEstimatedCovariance());
481         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
482         assertNull(calibrator.getGroundTruthGravityNorm());
483         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
484         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
485     }
486 
487     @Test
488     public void testConstructor6() throws WrongSizeException {
489         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
490                 new KnownBiasAndGravityNormAccelerometerCalibrator(
491                         true, this);
492 
493         // check default values
494         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
495         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
496         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
497         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
498         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
499         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
500         final Acceleration bx2 = new Acceleration(0.0,
501                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
502         calibrator.getBiasXAsAcceleration(bx2);
503         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
504         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
505         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
506         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
507         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
508         final Acceleration by2 = new Acceleration(0.0,
509                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
510         calibrator.getBiasYAsAcceleration(by2);
511         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
512         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
513         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
514         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
515         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
516         final Acceleration bz2 = new Acceleration(0.0,
517                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
518         calibrator.getBiasZAsAcceleration(bz2);
519         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
520         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
521         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
522         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
523         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
524         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
525         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
526         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
527         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
528         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
529         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
530         final double[] bias1 = calibrator.getBias();
531         assertArrayEquals(bias1, new double[3], 0.0);
532         final double[] bias2 = new double[3];
533         calibrator.getBias(bias2);
534         assertArrayEquals(bias1, bias2, 0.0);
535         final Matrix b1 = calibrator.getBiasAsMatrix();
536         assertEquals(b1, new Matrix(3, 1));
537         final Matrix b2 = new Matrix(3, 1);
538         calibrator.getBiasAsMatrix(b2);
539         assertEquals(b1, b2);
540         final Matrix ma1 = calibrator.getInitialMa();
541         assertEquals(ma1, new Matrix(3, 3));
542         final Matrix ma2 = new Matrix(3, 3);
543         calibrator.getInitialMa(ma2);
544         assertEquals(ma1, ma2);
545         assertNull(calibrator.getMeasurements());
546         assertTrue(calibrator.isCommonAxisUsed());
547         assertSame(calibrator.getListener(), this);
548         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
549         assertFalse(calibrator.isReady());
550         assertFalse(calibrator.isRunning());
551         assertNull(calibrator.getEstimatedMa());
552         assertNull(calibrator.getEstimatedSx());
553         assertNull(calibrator.getEstimatedSy());
554         assertNull(calibrator.getEstimatedSz());
555         assertNull(calibrator.getEstimatedMxy());
556         assertNull(calibrator.getEstimatedMxz());
557         assertNull(calibrator.getEstimatedMyx());
558         assertNull(calibrator.getEstimatedMyz());
559         assertNull(calibrator.getEstimatedMzx());
560         assertNull(calibrator.getEstimatedMzy());
561         assertNull(calibrator.getEstimatedCovariance());
562         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
563         assertNull(calibrator.getGroundTruthGravityNorm());
564         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
565         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
566     }
567 
568     @Test
569     public void testConstructor7() throws WrongSizeException {
570         final Collection<StandardDeviationBodyKinematics> measurements =
571                 Collections.emptyList();
572         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
573                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
574                         true);
575 
576         // check default values
577         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
578         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
579         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
580         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
581         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
582         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
583         final Acceleration bx2 = new Acceleration(0.0,
584                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
585         calibrator.getBiasXAsAcceleration(bx2);
586         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
587         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
588         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
589         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
590         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
591         final Acceleration by2 = new Acceleration(0.0,
592                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
593         calibrator.getBiasYAsAcceleration(by2);
594         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
595         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
596         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
597         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
598         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
599         final Acceleration bz2 = new Acceleration(0.0,
600                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
601         calibrator.getBiasZAsAcceleration(bz2);
602         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
603         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
604         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
605         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
606         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
607         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
608         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
609         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
610         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
611         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
612         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
613         final double[] bias1 = calibrator.getBias();
614         assertArrayEquals(bias1, new double[3], 0.0);
615         final double[] bias2 = new double[3];
616         calibrator.getBias(bias2);
617         assertArrayEquals(bias1, bias2, 0.0);
618         final Matrix b1 = calibrator.getBiasAsMatrix();
619         assertEquals(b1, new Matrix(3, 1));
620         final Matrix b2 = new Matrix(3, 1);
621         calibrator.getBiasAsMatrix(b2);
622         assertEquals(b1, b2);
623         final Matrix ma1 = calibrator.getInitialMa();
624         assertEquals(ma1, new Matrix(3, 3));
625         final Matrix ma2 = new Matrix(3, 3);
626         calibrator.getInitialMa(ma2);
627         assertEquals(ma1, ma2);
628         assertSame(calibrator.getMeasurements(), measurements);
629         assertTrue(calibrator.isCommonAxisUsed());
630         assertNull(calibrator.getListener());
631         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
632         assertFalse(calibrator.isReady());
633         assertFalse(calibrator.isRunning());
634         assertNull(calibrator.getEstimatedMa());
635         assertNull(calibrator.getEstimatedSx());
636         assertNull(calibrator.getEstimatedSy());
637         assertNull(calibrator.getEstimatedSz());
638         assertNull(calibrator.getEstimatedMxy());
639         assertNull(calibrator.getEstimatedMxz());
640         assertNull(calibrator.getEstimatedMyx());
641         assertNull(calibrator.getEstimatedMyz());
642         assertNull(calibrator.getEstimatedMzx());
643         assertNull(calibrator.getEstimatedMzy());
644         assertNull(calibrator.getEstimatedCovariance());
645         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
646         assertNull(calibrator.getGroundTruthGravityNorm());
647         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
648         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
649     }
650 
651     @Test
652     public void testConstructor8() throws WrongSizeException {
653         final Collection<StandardDeviationBodyKinematics> measurements =
654                 Collections.emptyList();
655         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
656                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
657                         true, this);
658 
659         // check default values
660         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
661         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
662         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
663         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
664         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
665         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
666         final Acceleration bx2 = new Acceleration(0.0,
667                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
668         calibrator.getBiasXAsAcceleration(bx2);
669         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
670         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
671         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
672         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
673         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
674         final Acceleration by2 = new Acceleration(0.0,
675                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
676         calibrator.getBiasYAsAcceleration(by2);
677         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
678         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
679         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
680         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
681         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
682         final Acceleration bz2 = new Acceleration(0.0,
683                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
684         calibrator.getBiasZAsAcceleration(bz2);
685         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
686         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
687         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
688         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
689         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
690         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
691         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
692         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
693         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
694         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
695         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
696         final double[] bias1 = calibrator.getBias();
697         assertArrayEquals(bias1, new double[3], 0.0);
698         final double[] bias2 = new double[3];
699         calibrator.getBias(bias2);
700         assertArrayEquals(bias1, bias2, 0.0);
701         final Matrix b1 = calibrator.getBiasAsMatrix();
702         assertEquals(b1, new Matrix(3, 1));
703         final Matrix b2 = new Matrix(3, 1);
704         calibrator.getBiasAsMatrix(b2);
705         assertEquals(b1, b2);
706         final Matrix ma1 = calibrator.getInitialMa();
707         assertEquals(ma1, new Matrix(3, 3));
708         final Matrix ma2 = new Matrix(3, 3);
709         calibrator.getInitialMa(ma2);
710         assertEquals(ma1, ma2);
711         assertSame(calibrator.getMeasurements(), measurements);
712         assertTrue(calibrator.isCommonAxisUsed());
713         assertSame(calibrator.getListener(), this);
714         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
715         assertFalse(calibrator.isReady());
716         assertFalse(calibrator.isRunning());
717         assertNull(calibrator.getEstimatedMa());
718         assertNull(calibrator.getEstimatedSx());
719         assertNull(calibrator.getEstimatedSy());
720         assertNull(calibrator.getEstimatedSz());
721         assertNull(calibrator.getEstimatedMxy());
722         assertNull(calibrator.getEstimatedMxz());
723         assertNull(calibrator.getEstimatedMyx());
724         assertNull(calibrator.getEstimatedMyz());
725         assertNull(calibrator.getEstimatedMzx());
726         assertNull(calibrator.getEstimatedMzy());
727         assertNull(calibrator.getEstimatedCovariance());
728         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
729         assertNull(calibrator.getGroundTruthGravityNorm());
730         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
731         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
732     }
733 
734     @Test
735     public void testConstructor9() throws WrongSizeException {
736         final Matrix ba = generateBa();
737         final double biasX = ba.getElementAtIndex(0);
738         final double biasY = ba.getElementAtIndex(1);
739         final double biasZ = ba.getElementAtIndex(2);
740 
741         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
742                 new KnownBiasAndGravityNormAccelerometerCalibrator(biasX, biasY, biasZ);
743 
744         // check default values
745         assertEquals(calibrator.getBiasX(), biasX, 0.0);
746         assertEquals(calibrator.getBiasY(), biasY, 0.0);
747         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
748         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
749         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
750         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
751         final Acceleration bx2 = new Acceleration(0.0,
752                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
753         calibrator.getBiasXAsAcceleration(bx2);
754         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
755         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
756         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
757         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
758         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
759         final Acceleration by2 = new Acceleration(0.0,
760                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
761         calibrator.getBiasYAsAcceleration(by2);
762         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
763         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
764         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
765         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
766         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
767         final Acceleration bz2 = new Acceleration(0.0,
768                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
769         calibrator.getBiasZAsAcceleration(bz2);
770         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
771         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
772         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
773         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
774         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
775         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
776         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
777         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
778         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
779         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
780         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
781         final double[] bias1 = calibrator.getBias();
782         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
783         final double[] bias2 = new double[3];
784         calibrator.getBias(bias2);
785         assertArrayEquals(bias1, bias2, 0.0);
786         final Matrix b1 = calibrator.getBiasAsMatrix();
787         assertEquals(b1, ba);
788         final Matrix b2 = new Matrix(3, 1);
789         calibrator.getBiasAsMatrix(b2);
790         assertEquals(b1, b2);
791         final Matrix ma1 = calibrator.getInitialMa();
792         assertEquals(ma1, new Matrix(3, 3));
793         final Matrix ma2 = new Matrix(3, 3);
794         calibrator.getInitialMa(ma2);
795         assertEquals(ma1, ma2);
796         assertNull(calibrator.getMeasurements());
797         assertFalse(calibrator.isCommonAxisUsed());
798         assertNull(calibrator.getListener());
799         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
800         assertFalse(calibrator.isReady());
801         assertFalse(calibrator.isRunning());
802         assertNull(calibrator.getEstimatedMa());
803         assertNull(calibrator.getEstimatedSx());
804         assertNull(calibrator.getEstimatedSy());
805         assertNull(calibrator.getEstimatedSz());
806         assertNull(calibrator.getEstimatedMxy());
807         assertNull(calibrator.getEstimatedMxz());
808         assertNull(calibrator.getEstimatedMyx());
809         assertNull(calibrator.getEstimatedMyz());
810         assertNull(calibrator.getEstimatedMzx());
811         assertNull(calibrator.getEstimatedMzy());
812         assertNull(calibrator.getEstimatedCovariance());
813         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
814         assertNull(calibrator.getGroundTruthGravityNorm());
815         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
816         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
817     }
818 
819     @Test
820     public void testConstructor10() throws WrongSizeException {
821         final Matrix ba = generateBa();
822         final double biasX = ba.getElementAtIndex(0);
823         final double biasY = ba.getElementAtIndex(1);
824         final double biasZ = ba.getElementAtIndex(2);
825 
826         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
827                 new KnownBiasAndGravityNormAccelerometerCalibrator(
828                         biasX, biasY, biasZ, this);
829 
830         // check default values
831         assertEquals(calibrator.getBiasX(), biasX, 0.0);
832         assertEquals(calibrator.getBiasY(), biasY, 0.0);
833         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
834         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
835         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
836         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
837         final Acceleration bx2 = new Acceleration(0.0,
838                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
839         calibrator.getBiasXAsAcceleration(bx2);
840         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
841         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
842         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
843         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
844         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
845         final Acceleration by2 = new Acceleration(0.0,
846                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
847         calibrator.getBiasYAsAcceleration(by2);
848         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
849         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
850         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
851         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
852         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
853         final Acceleration bz2 = new Acceleration(0.0,
854                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
855         calibrator.getBiasZAsAcceleration(bz2);
856         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
857         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
858         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
859         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
860         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
861         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
862         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
863         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
864         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
865         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
866         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
867         final double[] bias1 = calibrator.getBias();
868         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
869         final double[] bias2 = new double[3];
870         calibrator.getBias(bias2);
871         assertArrayEquals(bias1, bias2, 0.0);
872         final Matrix b1 = calibrator.getBiasAsMatrix();
873         assertEquals(b1, ba);
874         final Matrix b2 = new Matrix(3, 1);
875         calibrator.getBiasAsMatrix(b2);
876         assertEquals(b1, b2);
877         final Matrix ma1 = calibrator.getInitialMa();
878         assertEquals(ma1, new Matrix(3, 3));
879         final Matrix ma2 = new Matrix(3, 3);
880         calibrator.getInitialMa(ma2);
881         assertEquals(ma1, ma2);
882         assertNull(calibrator.getMeasurements());
883         assertFalse(calibrator.isCommonAxisUsed());
884         assertSame(calibrator.getListener(), this);
885         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
886         assertFalse(calibrator.isReady());
887         assertFalse(calibrator.isRunning());
888         assertNull(calibrator.getEstimatedMa());
889         assertNull(calibrator.getEstimatedSx());
890         assertNull(calibrator.getEstimatedSy());
891         assertNull(calibrator.getEstimatedSz());
892         assertNull(calibrator.getEstimatedMxy());
893         assertNull(calibrator.getEstimatedMxz());
894         assertNull(calibrator.getEstimatedMyx());
895         assertNull(calibrator.getEstimatedMyz());
896         assertNull(calibrator.getEstimatedMzx());
897         assertNull(calibrator.getEstimatedMzy());
898         assertNull(calibrator.getEstimatedCovariance());
899         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
900         assertNull(calibrator.getGroundTruthGravityNorm());
901         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
902         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
903     }
904 
905     @Test
906     public void testConstructor11() throws WrongSizeException {
907         final Collection<StandardDeviationBodyKinematics> measurements =
908                 Collections.emptyList();
909 
910         final Matrix ba = generateBa();
911         final double biasX = ba.getElementAtIndex(0);
912         final double biasY = ba.getElementAtIndex(1);
913         final double biasZ = ba.getElementAtIndex(2);
914 
915         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
916                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
917                         biasX, biasY, biasZ);
918 
919         // check default values
920         assertEquals(calibrator.getBiasX(), biasX, 0.0);
921         assertEquals(calibrator.getBiasY(), biasY, 0.0);
922         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
923         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
924         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
925         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
926         final Acceleration bx2 = new Acceleration(0.0,
927                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
928         calibrator.getBiasXAsAcceleration(bx2);
929         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
930         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
931         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
932         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
933         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
934         final Acceleration by2 = new Acceleration(0.0,
935                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
936         calibrator.getBiasYAsAcceleration(by2);
937         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
938         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
939         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
940         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
941         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
942         final Acceleration bz2 = new Acceleration(0.0,
943                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
944         calibrator.getBiasZAsAcceleration(bz2);
945         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
946         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
947         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
948         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
949         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
950         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
951         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
952         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
953         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
954         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
955         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
956         final double[] bias1 = calibrator.getBias();
957         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
958         final double[] bias2 = new double[3];
959         calibrator.getBias(bias2);
960         assertArrayEquals(bias1, bias2, 0.0);
961         final Matrix b1 = calibrator.getBiasAsMatrix();
962         assertEquals(b1, ba);
963         final Matrix b2 = new Matrix(3, 1);
964         calibrator.getBiasAsMatrix(b2);
965         assertEquals(b1, b2);
966         final Matrix ma1 = calibrator.getInitialMa();
967         assertEquals(ma1, new Matrix(3, 3));
968         final Matrix ma2 = new Matrix(3, 3);
969         calibrator.getInitialMa(ma2);
970         assertEquals(ma1, ma2);
971         assertSame(calibrator.getMeasurements(), measurements);
972         assertFalse(calibrator.isCommonAxisUsed());
973         assertNull(calibrator.getListener());
974         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
975         assertFalse(calibrator.isReady());
976         assertFalse(calibrator.isRunning());
977         assertNull(calibrator.getEstimatedMa());
978         assertNull(calibrator.getEstimatedSx());
979         assertNull(calibrator.getEstimatedSy());
980         assertNull(calibrator.getEstimatedSz());
981         assertNull(calibrator.getEstimatedMxy());
982         assertNull(calibrator.getEstimatedMxz());
983         assertNull(calibrator.getEstimatedMyx());
984         assertNull(calibrator.getEstimatedMyz());
985         assertNull(calibrator.getEstimatedMzx());
986         assertNull(calibrator.getEstimatedMzy());
987         assertNull(calibrator.getEstimatedCovariance());
988         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
989         assertNull(calibrator.getGroundTruthGravityNorm());
990         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
991         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
992     }
993 
994     @Test
995     public void testConstructor12() throws WrongSizeException {
996         final Collection<StandardDeviationBodyKinematics> measurements =
997                 Collections.emptyList();
998 
999         final Matrix ba = generateBa();
1000         final double biasX = ba.getElementAtIndex(0);
1001         final double biasY = ba.getElementAtIndex(1);
1002         final double biasZ = ba.getElementAtIndex(2);
1003 
1004         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1005                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
1006                         biasX, biasY, biasZ, this);
1007 
1008         // check default values
1009         assertEquals(calibrator.getBiasX(), biasX, 0.0);
1010         assertEquals(calibrator.getBiasY(), biasY, 0.0);
1011         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1012         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1013         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1014         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1015         final Acceleration bx2 = new Acceleration(0.0,
1016                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1017         calibrator.getBiasXAsAcceleration(bx2);
1018         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1019         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1020         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1021         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1022         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1023         final Acceleration by2 = new Acceleration(0.0,
1024                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1025         calibrator.getBiasYAsAcceleration(by2);
1026         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1027         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1028         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1029         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1030         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1031         final Acceleration bz2 = new Acceleration(0.0,
1032                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1033         calibrator.getBiasZAsAcceleration(bz2);
1034         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1035         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1036         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1037         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1038         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1039         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1040         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1041         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1042         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1043         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1044         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1045         final double[] bias1 = calibrator.getBias();
1046         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1047         final double[] bias2 = new double[3];
1048         calibrator.getBias(bias2);
1049         assertArrayEquals(bias1, bias2, 0.0);
1050         final Matrix b1 = calibrator.getBiasAsMatrix();
1051         assertEquals(b1, ba);
1052         final Matrix b2 = new Matrix(3, 1);
1053         calibrator.getBiasAsMatrix(b2);
1054         assertEquals(b1, b2);
1055         final Matrix ma1 = calibrator.getInitialMa();
1056         assertEquals(ma1, new Matrix(3, 3));
1057         final Matrix ma2 = new Matrix(3, 3);
1058         calibrator.getInitialMa(ma2);
1059         assertEquals(ma1, ma2);
1060         assertSame(calibrator.getMeasurements(), measurements);
1061         assertFalse(calibrator.isCommonAxisUsed());
1062         assertSame(calibrator.getListener(), this);
1063         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1064         assertFalse(calibrator.isReady());
1065         assertFalse(calibrator.isRunning());
1066         assertNull(calibrator.getEstimatedMa());
1067         assertNull(calibrator.getEstimatedSx());
1068         assertNull(calibrator.getEstimatedSy());
1069         assertNull(calibrator.getEstimatedSz());
1070         assertNull(calibrator.getEstimatedMxy());
1071         assertNull(calibrator.getEstimatedMxz());
1072         assertNull(calibrator.getEstimatedMyx());
1073         assertNull(calibrator.getEstimatedMyz());
1074         assertNull(calibrator.getEstimatedMzx());
1075         assertNull(calibrator.getEstimatedMzy());
1076         assertNull(calibrator.getEstimatedCovariance());
1077         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1078         assertNull(calibrator.getGroundTruthGravityNorm());
1079         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1080         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1081     }
1082 
1083     @Test
1084     public void testConstructor13() throws WrongSizeException {
1085         final Matrix ba = generateBa();
1086         final double biasX = ba.getElementAtIndex(0);
1087         final double biasY = ba.getElementAtIndex(1);
1088         final double biasZ = ba.getElementAtIndex(2);
1089 
1090         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1091                 new KnownBiasAndGravityNormAccelerometerCalibrator(
1092                         true, biasX, biasY, biasZ);
1093 
1094         // check default values
1095         assertEquals(calibrator.getBiasX(), biasX, 0.0);
1096         assertEquals(calibrator.getBiasY(), biasY, 0.0);
1097         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1098         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1099         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1100         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1101         final Acceleration bx2 = new Acceleration(0.0,
1102                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1103         calibrator.getBiasXAsAcceleration(bx2);
1104         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1105         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1106         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1107         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1108         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1109         final Acceleration by2 = new Acceleration(0.0,
1110                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1111         calibrator.getBiasYAsAcceleration(by2);
1112         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1113         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1114         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1115         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1116         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1117         final Acceleration bz2 = new Acceleration(0.0,
1118                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1119         calibrator.getBiasZAsAcceleration(bz2);
1120         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1121         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1122         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1123         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1124         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1125         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1126         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1127         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1128         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1129         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1130         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1131         final double[] bias1 = calibrator.getBias();
1132         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1133         final double[] bias2 = new double[3];
1134         calibrator.getBias(bias2);
1135         assertArrayEquals(bias1, bias2, 0.0);
1136         final Matrix b1 = calibrator.getBiasAsMatrix();
1137         assertEquals(b1, ba);
1138         final Matrix b2 = new Matrix(3, 1);
1139         calibrator.getBiasAsMatrix(b2);
1140         assertEquals(b1, b2);
1141         final Matrix ma1 = calibrator.getInitialMa();
1142         assertEquals(ma1, new Matrix(3, 3));
1143         final Matrix ma2 = new Matrix(3, 3);
1144         calibrator.getInitialMa(ma2);
1145         assertEquals(ma1, ma2);
1146         assertNull(calibrator.getMeasurements());
1147         assertTrue(calibrator.isCommonAxisUsed());
1148         assertNull(calibrator.getListener());
1149         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
1150         assertFalse(calibrator.isReady());
1151         assertFalse(calibrator.isRunning());
1152         assertNull(calibrator.getEstimatedMa());
1153         assertNull(calibrator.getEstimatedSx());
1154         assertNull(calibrator.getEstimatedSy());
1155         assertNull(calibrator.getEstimatedSz());
1156         assertNull(calibrator.getEstimatedMxy());
1157         assertNull(calibrator.getEstimatedMxz());
1158         assertNull(calibrator.getEstimatedMyx());
1159         assertNull(calibrator.getEstimatedMyz());
1160         assertNull(calibrator.getEstimatedMzx());
1161         assertNull(calibrator.getEstimatedMzy());
1162         assertNull(calibrator.getEstimatedCovariance());
1163         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1164         assertNull(calibrator.getGroundTruthGravityNorm());
1165         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1166         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1167     }
1168 
1169     @Test
1170     public void testConstructor14() throws WrongSizeException {
1171         final Matrix ba = generateBa();
1172         final double biasX = ba.getElementAtIndex(0);
1173         final double biasY = ba.getElementAtIndex(1);
1174         final double biasZ = ba.getElementAtIndex(2);
1175 
1176         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1177                 new KnownBiasAndGravityNormAccelerometerCalibrator(
1178                         true, biasX, biasY, biasZ,
1179                         this);
1180 
1181         // check default values
1182         assertEquals(calibrator.getBiasX(), biasX, 0.0);
1183         assertEquals(calibrator.getBiasY(), biasY, 0.0);
1184         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1185         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1186         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1187         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1188         final Acceleration bx2 = new Acceleration(0.0,
1189                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1190         calibrator.getBiasXAsAcceleration(bx2);
1191         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1192         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1193         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1194         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1195         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1196         final Acceleration by2 = new Acceleration(0.0,
1197                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1198         calibrator.getBiasYAsAcceleration(by2);
1199         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1200         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1201         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1202         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1203         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1204         final Acceleration bz2 = new Acceleration(0.0,
1205                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1206         calibrator.getBiasZAsAcceleration(bz2);
1207         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1208         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1209         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1210         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1211         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1212         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1213         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1214         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1215         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1216         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1217         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1218         final double[] bias1 = calibrator.getBias();
1219         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1220         final double[] bias2 = new double[3];
1221         calibrator.getBias(bias2);
1222         assertArrayEquals(bias1, bias2, 0.0);
1223         final Matrix b1 = calibrator.getBiasAsMatrix();
1224         assertEquals(b1, ba);
1225         final Matrix b2 = new Matrix(3, 1);
1226         calibrator.getBiasAsMatrix(b2);
1227         assertEquals(b1, b2);
1228         final Matrix ma1 = calibrator.getInitialMa();
1229         assertEquals(ma1, new Matrix(3, 3));
1230         final Matrix ma2 = new Matrix(3, 3);
1231         calibrator.getInitialMa(ma2);
1232         assertEquals(ma1, ma2);
1233         assertNull(calibrator.getMeasurements());
1234         assertTrue(calibrator.isCommonAxisUsed());
1235         assertSame(calibrator.getListener(), this);
1236         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
1237         assertFalse(calibrator.isReady());
1238         assertFalse(calibrator.isRunning());
1239         assertNull(calibrator.getEstimatedMa());
1240         assertNull(calibrator.getEstimatedSx());
1241         assertNull(calibrator.getEstimatedSy());
1242         assertNull(calibrator.getEstimatedSz());
1243         assertNull(calibrator.getEstimatedMxy());
1244         assertNull(calibrator.getEstimatedMxz());
1245         assertNull(calibrator.getEstimatedMyx());
1246         assertNull(calibrator.getEstimatedMyz());
1247         assertNull(calibrator.getEstimatedMzx());
1248         assertNull(calibrator.getEstimatedMzy());
1249         assertNull(calibrator.getEstimatedCovariance());
1250         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1251         assertNull(calibrator.getGroundTruthGravityNorm());
1252         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1253         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1254     }
1255 
1256     @Test
1257     public void testConstructor15() throws WrongSizeException {
1258         final Collection<StandardDeviationBodyKinematics> measurements =
1259                 Collections.emptyList();
1260 
1261         final Matrix ba = generateBa();
1262         final double biasX = ba.getElementAtIndex(0);
1263         final double biasY = ba.getElementAtIndex(1);
1264         final double biasZ = ba.getElementAtIndex(2);
1265 
1266         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1267                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
1268                         true, biasX, biasY, biasZ);
1269 
1270         // check default values
1271         assertEquals(calibrator.getBiasX(), biasX, 0.0);
1272         assertEquals(calibrator.getBiasY(), biasY, 0.0);
1273         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1274         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1275         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1276         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1277         final Acceleration bx2 = new Acceleration(0.0,
1278                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1279         calibrator.getBiasXAsAcceleration(bx2);
1280         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1281         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1282         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1283         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1284         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1285         final Acceleration by2 = new Acceleration(0.0,
1286                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1287         calibrator.getBiasYAsAcceleration(by2);
1288         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1289         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1290         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1291         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1292         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1293         final Acceleration bz2 = new Acceleration(0.0,
1294                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1295         calibrator.getBiasZAsAcceleration(bz2);
1296         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1297         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1298         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1299         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1300         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1301         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1302         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1303         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1304         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1305         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1306         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1307         final double[] bias1 = calibrator.getBias();
1308         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1309         final double[] bias2 = new double[3];
1310         calibrator.getBias(bias2);
1311         assertArrayEquals(bias1, bias2, 0.0);
1312         final Matrix b1 = calibrator.getBiasAsMatrix();
1313         assertEquals(b1, ba);
1314         final Matrix b2 = new Matrix(3, 1);
1315         calibrator.getBiasAsMatrix(b2);
1316         assertEquals(b1, b2);
1317         final Matrix ma1 = calibrator.getInitialMa();
1318         assertEquals(ma1, new Matrix(3, 3));
1319         final Matrix ma2 = new Matrix(3, 3);
1320         calibrator.getInitialMa(ma2);
1321         assertEquals(ma1, ma2);
1322         assertSame(calibrator.getMeasurements(), measurements);
1323         assertTrue(calibrator.isCommonAxisUsed());
1324         assertNull(calibrator.getListener());
1325         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
1326         assertFalse(calibrator.isReady());
1327         assertFalse(calibrator.isRunning());
1328         assertNull(calibrator.getEstimatedMa());
1329         assertNull(calibrator.getEstimatedSx());
1330         assertNull(calibrator.getEstimatedSy());
1331         assertNull(calibrator.getEstimatedSz());
1332         assertNull(calibrator.getEstimatedMxy());
1333         assertNull(calibrator.getEstimatedMxz());
1334         assertNull(calibrator.getEstimatedMyx());
1335         assertNull(calibrator.getEstimatedMyz());
1336         assertNull(calibrator.getEstimatedMzx());
1337         assertNull(calibrator.getEstimatedMzy());
1338         assertNull(calibrator.getEstimatedCovariance());
1339         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1340         assertNull(calibrator.getGroundTruthGravityNorm());
1341         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1342         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1343     }
1344 
1345     @Test
1346     public void testConstructor16() throws WrongSizeException {
1347         final Collection<StandardDeviationBodyKinematics> measurements =
1348                 Collections.emptyList();
1349 
1350         final Matrix ba = generateBa();
1351         final double biasX = ba.getElementAtIndex(0);
1352         final double biasY = ba.getElementAtIndex(1);
1353         final double biasZ = ba.getElementAtIndex(2);
1354 
1355         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1356                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
1357                         true, biasX, biasY, biasZ, this);
1358 
1359         // check default values
1360         assertEquals(calibrator.getBiasX(), biasX, 0.0);
1361         assertEquals(calibrator.getBiasY(), biasY, 0.0);
1362         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1363         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1364         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1365         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1366         final Acceleration bx2 = new Acceleration(0.0,
1367                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1368         calibrator.getBiasXAsAcceleration(bx2);
1369         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1370         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1371         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1372         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1373         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1374         final Acceleration by2 = new Acceleration(0.0,
1375                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1376         calibrator.getBiasYAsAcceleration(by2);
1377         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1378         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1379         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1380         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1381         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1382         final Acceleration bz2 = new Acceleration(0.0,
1383                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1384         calibrator.getBiasZAsAcceleration(bz2);
1385         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1386         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1387         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1388         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1389         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1390         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1391         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1392         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1393         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1394         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1395         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1396         final double[] bias1 = calibrator.getBias();
1397         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1398         final double[] bias2 = new double[3];
1399         calibrator.getBias(bias2);
1400         assertArrayEquals(bias1, bias2, 0.0);
1401         final Matrix b1 = calibrator.getBiasAsMatrix();
1402         assertEquals(b1, ba);
1403         final Matrix b2 = new Matrix(3, 1);
1404         calibrator.getBiasAsMatrix(b2);
1405         assertEquals(b1, b2);
1406         final Matrix ma1 = calibrator.getInitialMa();
1407         assertEquals(ma1, new Matrix(3, 3));
1408         final Matrix ma2 = new Matrix(3, 3);
1409         calibrator.getInitialMa(ma2);
1410         assertEquals(ma1, ma2);
1411         assertSame(calibrator.getMeasurements(), measurements);
1412         assertTrue(calibrator.isCommonAxisUsed());
1413         assertSame(calibrator.getListener(), this);
1414         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
1415         assertFalse(calibrator.isReady());
1416         assertFalse(calibrator.isRunning());
1417         assertNull(calibrator.getEstimatedMa());
1418         assertNull(calibrator.getEstimatedSx());
1419         assertNull(calibrator.getEstimatedSy());
1420         assertNull(calibrator.getEstimatedSz());
1421         assertNull(calibrator.getEstimatedMxy());
1422         assertNull(calibrator.getEstimatedMxz());
1423         assertNull(calibrator.getEstimatedMyx());
1424         assertNull(calibrator.getEstimatedMyz());
1425         assertNull(calibrator.getEstimatedMzx());
1426         assertNull(calibrator.getEstimatedMzy());
1427         assertNull(calibrator.getEstimatedCovariance());
1428         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1429         assertNull(calibrator.getGroundTruthGravityNorm());
1430         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1431         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1432     }
1433 
1434     @Test
1435     public void testConstructor17() throws WrongSizeException {
1436         final Matrix ba = generateBa();
1437         final double biasX = ba.getElementAtIndex(0);
1438         final double biasY = ba.getElementAtIndex(1);
1439         final double biasZ = ba.getElementAtIndex(2);
1440 
1441         final Acceleration bx = new Acceleration(biasX,
1442                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1443         final Acceleration by = new Acceleration(biasY,
1444                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1445         final Acceleration bz = new Acceleration(biasZ,
1446                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1447 
1448         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1449                 new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz);
1450 
1451         // check default values
1452         assertEquals(calibrator.getBiasX(), biasX, 0.0);
1453         assertEquals(calibrator.getBiasY(), biasY, 0.0);
1454         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1455         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1456         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1457         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1458         final Acceleration bx2 = new Acceleration(0.0,
1459                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1460         calibrator.getBiasXAsAcceleration(bx2);
1461         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1462         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1463         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1464         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1465         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1466         final Acceleration by2 = new Acceleration(0.0,
1467                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1468         calibrator.getBiasYAsAcceleration(by2);
1469         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1470         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1471         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1472         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1473         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1474         final Acceleration bz2 = new Acceleration(0.0,
1475                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1476         calibrator.getBiasZAsAcceleration(bz2);
1477         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1478         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1479         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1480         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1481         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1482         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1483         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1484         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1485         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1486         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1487         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1488         final double[] bias1 = calibrator.getBias();
1489         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1490         final double[] bias2 = new double[3];
1491         calibrator.getBias(bias2);
1492         assertArrayEquals(bias1, bias2, 0.0);
1493         final Matrix b1 = calibrator.getBiasAsMatrix();
1494         assertEquals(b1, ba);
1495         final Matrix b2 = new Matrix(3, 1);
1496         calibrator.getBiasAsMatrix(b2);
1497         assertEquals(b1, b2);
1498         final Matrix ma1 = calibrator.getInitialMa();
1499         assertEquals(ma1, new Matrix(3, 3));
1500         final Matrix ma2 = new Matrix(3, 3);
1501         calibrator.getInitialMa(ma2);
1502         assertEquals(ma1, ma2);
1503         assertNull(calibrator.getMeasurements());
1504         assertFalse(calibrator.isCommonAxisUsed());
1505         assertNull(calibrator.getListener());
1506         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1507         assertFalse(calibrator.isReady());
1508         assertFalse(calibrator.isRunning());
1509         assertNull(calibrator.getEstimatedMa());
1510         assertNull(calibrator.getEstimatedSx());
1511         assertNull(calibrator.getEstimatedSy());
1512         assertNull(calibrator.getEstimatedSz());
1513         assertNull(calibrator.getEstimatedMxy());
1514         assertNull(calibrator.getEstimatedMxz());
1515         assertNull(calibrator.getEstimatedMyx());
1516         assertNull(calibrator.getEstimatedMyz());
1517         assertNull(calibrator.getEstimatedMzx());
1518         assertNull(calibrator.getEstimatedMzy());
1519         assertNull(calibrator.getEstimatedCovariance());
1520         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1521         assertNull(calibrator.getGroundTruthGravityNorm());
1522         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1523         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1524     }
1525 
1526     @Test
1527     public void testConstructor18() throws WrongSizeException {
1528         final Matrix ba = generateBa();
1529         final double biasX = ba.getElementAtIndex(0);
1530         final double biasY = ba.getElementAtIndex(1);
1531         final double biasZ = ba.getElementAtIndex(2);
1532 
1533         final Acceleration bx = new Acceleration(biasX,
1534                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1535         final Acceleration by = new Acceleration(biasY,
1536                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1537         final Acceleration bz = new Acceleration(biasZ,
1538                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1539 
1540         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1541                 new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz,
1542                         this);
1543 
1544         // check default values
1545         assertEquals(calibrator.getBiasX(), biasX, 0.0);
1546         assertEquals(calibrator.getBiasY(), biasY, 0.0);
1547         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1548         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1549         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1550         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1551         final Acceleration bx2 = new Acceleration(0.0,
1552                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1553         calibrator.getBiasXAsAcceleration(bx2);
1554         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1555         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1556         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1557         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1558         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1559         final Acceleration by2 = new Acceleration(0.0,
1560                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1561         calibrator.getBiasYAsAcceleration(by2);
1562         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1563         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1564         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1565         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1566         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1567         final Acceleration bz2 = new Acceleration(0.0,
1568                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1569         calibrator.getBiasZAsAcceleration(bz2);
1570         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1571         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1572         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1573         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1574         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1575         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1576         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1577         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1578         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1579         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1580         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1581         final double[] bias1 = calibrator.getBias();
1582         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1583         final double[] bias2 = new double[3];
1584         calibrator.getBias(bias2);
1585         assertArrayEquals(bias1, bias2, 0.0);
1586         final Matrix b1 = calibrator.getBiasAsMatrix();
1587         assertEquals(b1, ba);
1588         final Matrix b2 = new Matrix(3, 1);
1589         calibrator.getBiasAsMatrix(b2);
1590         assertEquals(b1, b2);
1591         final Matrix ma1 = calibrator.getInitialMa();
1592         assertEquals(ma1, new Matrix(3, 3));
1593         final Matrix ma2 = new Matrix(3, 3);
1594         calibrator.getInitialMa(ma2);
1595         assertEquals(ma1, ma2);
1596         assertNull(calibrator.getMeasurements());
1597         assertFalse(calibrator.isCommonAxisUsed());
1598         assertSame(calibrator.getListener(), this);
1599         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1600         assertFalse(calibrator.isReady());
1601         assertFalse(calibrator.isRunning());
1602         assertNull(calibrator.getEstimatedMa());
1603         assertNull(calibrator.getEstimatedSx());
1604         assertNull(calibrator.getEstimatedSy());
1605         assertNull(calibrator.getEstimatedSz());
1606         assertNull(calibrator.getEstimatedMxy());
1607         assertNull(calibrator.getEstimatedMxz());
1608         assertNull(calibrator.getEstimatedMyx());
1609         assertNull(calibrator.getEstimatedMyz());
1610         assertNull(calibrator.getEstimatedMzx());
1611         assertNull(calibrator.getEstimatedMzy());
1612         assertNull(calibrator.getEstimatedCovariance());
1613         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1614         assertNull(calibrator.getGroundTruthGravityNorm());
1615         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1616         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1617     }
1618 
1619     @Test
1620     public void testConstructor19() throws WrongSizeException {
1621         final Collection<StandardDeviationBodyKinematics> measurements =
1622                 Collections.emptyList();
1623 
1624         final Matrix ba = generateBa();
1625         final double biasX = ba.getElementAtIndex(0);
1626         final double biasY = ba.getElementAtIndex(1);
1627         final double biasZ = ba.getElementAtIndex(2);
1628 
1629         final Acceleration bx = new Acceleration(biasX,
1630                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1631         final Acceleration by = new Acceleration(biasY,
1632                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1633         final Acceleration bz = new Acceleration(biasZ,
1634                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1635 
1636         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1637                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
1638                         bx, by, bz);
1639 
1640         // check default values
1641         assertEquals(calibrator.getBiasX(), biasX, 0.0);
1642         assertEquals(calibrator.getBiasY(), biasY, 0.0);
1643         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1644         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1645         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1646         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1647         final Acceleration bx2 = new Acceleration(0.0,
1648                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1649         calibrator.getBiasXAsAcceleration(bx2);
1650         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1651         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1652         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1653         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1654         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1655         final Acceleration by2 = new Acceleration(0.0,
1656                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1657         calibrator.getBiasYAsAcceleration(by2);
1658         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1659         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1660         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1661         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1662         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1663         final Acceleration bz2 = new Acceleration(0.0,
1664                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1665         calibrator.getBiasZAsAcceleration(bz2);
1666         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1667         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1668         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1669         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1670         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1671         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1672         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1673         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1674         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1675         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1676         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1677         final double[] bias1 = calibrator.getBias();
1678         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1679         final double[] bias2 = new double[3];
1680         calibrator.getBias(bias2);
1681         assertArrayEquals(bias1, bias2, 0.0);
1682         final Matrix b1 = calibrator.getBiasAsMatrix();
1683         assertEquals(b1, ba);
1684         final Matrix b2 = new Matrix(3, 1);
1685         calibrator.getBiasAsMatrix(b2);
1686         assertEquals(b1, b2);
1687         final Matrix ma1 = calibrator.getInitialMa();
1688         assertEquals(ma1, new Matrix(3, 3));
1689         final Matrix ma2 = new Matrix(3, 3);
1690         calibrator.getInitialMa(ma2);
1691         assertEquals(ma1, ma2);
1692         assertSame(calibrator.getMeasurements(), measurements);
1693         assertFalse(calibrator.isCommonAxisUsed());
1694         assertNull(calibrator.getListener());
1695         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1696         assertFalse(calibrator.isReady());
1697         assertFalse(calibrator.isRunning());
1698         assertNull(calibrator.getEstimatedMa());
1699         assertNull(calibrator.getEstimatedSx());
1700         assertNull(calibrator.getEstimatedSy());
1701         assertNull(calibrator.getEstimatedSz());
1702         assertNull(calibrator.getEstimatedMxy());
1703         assertNull(calibrator.getEstimatedMxz());
1704         assertNull(calibrator.getEstimatedMyx());
1705         assertNull(calibrator.getEstimatedMyz());
1706         assertNull(calibrator.getEstimatedMzx());
1707         assertNull(calibrator.getEstimatedMzy());
1708         assertNull(calibrator.getEstimatedCovariance());
1709         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1710         assertNull(calibrator.getGroundTruthGravityNorm());
1711         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1712         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1713     }
1714 
1715     @Test
1716     public void testConstructor20() throws WrongSizeException {
1717         final Collection<StandardDeviationBodyKinematics> measurements =
1718                 Collections.emptyList();
1719 
1720         final Matrix ba = generateBa();
1721         final double biasX = ba.getElementAtIndex(0);
1722         final double biasY = ba.getElementAtIndex(1);
1723         final double biasZ = ba.getElementAtIndex(2);
1724 
1725         final Acceleration bx = new Acceleration(biasX,
1726                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1727         final Acceleration by = new Acceleration(biasY,
1728                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1729         final Acceleration bz = new Acceleration(biasZ,
1730                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1731 
1732         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1733                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
1734                         bx, by, bz, this);
1735 
1736         // check default values
1737         assertEquals(calibrator.getBiasX(), biasX, 0.0);
1738         assertEquals(calibrator.getBiasY(), biasY, 0.0);
1739         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1740         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1741         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1742         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1743         final Acceleration bx2 = new Acceleration(0.0,
1744                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1745         calibrator.getBiasXAsAcceleration(bx2);
1746         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1747         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1748         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1749         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1750         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1751         final Acceleration by2 = new Acceleration(0.0,
1752                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1753         calibrator.getBiasYAsAcceleration(by2);
1754         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1755         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1756         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1757         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1758         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1759         final Acceleration bz2 = new Acceleration(0.0,
1760                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1761         calibrator.getBiasZAsAcceleration(bz2);
1762         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1763         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1764         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1765         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1766         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1767         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1768         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1769         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1770         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1771         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1772         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1773         final double[] bias1 = calibrator.getBias();
1774         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1775         final double[] bias2 = new double[3];
1776         calibrator.getBias(bias2);
1777         assertArrayEquals(bias1, bias2, 0.0);
1778         final Matrix b1 = calibrator.getBiasAsMatrix();
1779         assertEquals(b1, ba);
1780         final Matrix b2 = new Matrix(3, 1);
1781         calibrator.getBiasAsMatrix(b2);
1782         assertEquals(b1, b2);
1783         final Matrix ma1 = calibrator.getInitialMa();
1784         assertEquals(ma1, new Matrix(3, 3));
1785         final Matrix ma2 = new Matrix(3, 3);
1786         calibrator.getInitialMa(ma2);
1787         assertEquals(ma1, ma2);
1788         assertSame(calibrator.getMeasurements(), measurements);
1789         assertFalse(calibrator.isCommonAxisUsed());
1790         assertSame(calibrator.getListener(), this);
1791         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1792         assertFalse(calibrator.isReady());
1793         assertFalse(calibrator.isRunning());
1794         assertNull(calibrator.getEstimatedMa());
1795         assertNull(calibrator.getEstimatedSx());
1796         assertNull(calibrator.getEstimatedSy());
1797         assertNull(calibrator.getEstimatedSz());
1798         assertNull(calibrator.getEstimatedMxy());
1799         assertNull(calibrator.getEstimatedMxz());
1800         assertNull(calibrator.getEstimatedMyx());
1801         assertNull(calibrator.getEstimatedMyz());
1802         assertNull(calibrator.getEstimatedMzx());
1803         assertNull(calibrator.getEstimatedMzy());
1804         assertNull(calibrator.getEstimatedCovariance());
1805         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1806         assertNull(calibrator.getGroundTruthGravityNorm());
1807         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1808         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1809     }
1810 
1811     @Test
1812     public void testConstructor21() throws WrongSizeException {
1813         final Matrix ba = generateBa();
1814         final double biasX = ba.getElementAtIndex(0);
1815         final double biasY = ba.getElementAtIndex(1);
1816         final double biasZ = ba.getElementAtIndex(2);
1817 
1818         final Acceleration bx = new Acceleration(biasX,
1819                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1820         final Acceleration by = new Acceleration(biasY,
1821                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1822         final Acceleration bz = new Acceleration(biasZ,
1823                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1824 
1825         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1826                 new KnownBiasAndGravityNormAccelerometerCalibrator(
1827                         true, bx, by, bz);
1828 
1829         // check default values
1830         assertEquals(calibrator.getBiasX(), biasX, 0.0);
1831         assertEquals(calibrator.getBiasY(), biasY, 0.0);
1832         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1833         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1834         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1835         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1836         final Acceleration bx2 = new Acceleration(0.0,
1837                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1838         calibrator.getBiasXAsAcceleration(bx2);
1839         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1840         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1841         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1842         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1843         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1844         final Acceleration by2 = new Acceleration(0.0,
1845                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1846         calibrator.getBiasYAsAcceleration(by2);
1847         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1848         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1849         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1850         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1851         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1852         final Acceleration bz2 = new Acceleration(0.0,
1853                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1854         calibrator.getBiasZAsAcceleration(bz2);
1855         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1856         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1857         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1858         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1859         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1860         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1861         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1862         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1863         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1864         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1865         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1866         final double[] bias1 = calibrator.getBias();
1867         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1868         final double[] bias2 = new double[3];
1869         calibrator.getBias(bias2);
1870         assertArrayEquals(bias1, bias2, 0.0);
1871         final Matrix b1 = calibrator.getBiasAsMatrix();
1872         assertEquals(b1, ba);
1873         final Matrix b2 = new Matrix(3, 1);
1874         calibrator.getBiasAsMatrix(b2);
1875         assertEquals(b1, b2);
1876         final Matrix ma1 = calibrator.getInitialMa();
1877         assertEquals(ma1, new Matrix(3, 3));
1878         final Matrix ma2 = new Matrix(3, 3);
1879         calibrator.getInitialMa(ma2);
1880         assertEquals(ma1, ma2);
1881         assertNull(calibrator.getMeasurements());
1882         assertTrue(calibrator.isCommonAxisUsed());
1883         assertNull(calibrator.getListener());
1884         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
1885         assertFalse(calibrator.isReady());
1886         assertFalse(calibrator.isRunning());
1887         assertNull(calibrator.getEstimatedMa());
1888         assertNull(calibrator.getEstimatedSx());
1889         assertNull(calibrator.getEstimatedSy());
1890         assertNull(calibrator.getEstimatedSz());
1891         assertNull(calibrator.getEstimatedMxy());
1892         assertNull(calibrator.getEstimatedMxz());
1893         assertNull(calibrator.getEstimatedMyx());
1894         assertNull(calibrator.getEstimatedMyz());
1895         assertNull(calibrator.getEstimatedMzx());
1896         assertNull(calibrator.getEstimatedMzy());
1897         assertNull(calibrator.getEstimatedCovariance());
1898         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1899         assertNull(calibrator.getGroundTruthGravityNorm());
1900         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1901         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1902     }
1903 
1904     @Test
1905     public void testConstructor22() throws WrongSizeException {
1906         final Matrix ba = generateBa();
1907         final double biasX = ba.getElementAtIndex(0);
1908         final double biasY = ba.getElementAtIndex(1);
1909         final double biasZ = ba.getElementAtIndex(2);
1910 
1911         final Acceleration bx = new Acceleration(biasX,
1912                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1913         final Acceleration by = new Acceleration(biasY,
1914                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1915         final Acceleration bz = new Acceleration(biasZ,
1916                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1917 
1918         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
1919                 new KnownBiasAndGravityNormAccelerometerCalibrator(
1920                         true, bx, by, bz, this);
1921 
1922         // check default values
1923         assertEquals(calibrator.getBiasX(), biasX, 0.0);
1924         assertEquals(calibrator.getBiasY(), biasY, 0.0);
1925         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1926         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
1927         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1928         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1929         final Acceleration bx2 = new Acceleration(0.0,
1930                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1931         calibrator.getBiasXAsAcceleration(bx2);
1932         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1933         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1934         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
1935         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1936         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1937         final Acceleration by2 = new Acceleration(0.0,
1938                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1939         calibrator.getBiasYAsAcceleration(by2);
1940         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1941         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1942         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
1943         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1944         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1945         final Acceleration bz2 = new Acceleration(0.0,
1946                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1947         calibrator.getBiasZAsAcceleration(bz2);
1948         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1949         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1950         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1951         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1952         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1953         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1954         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1955         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1956         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1957         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1958         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1959         final double[] bias1 = calibrator.getBias();
1960         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1961         final double[] bias2 = new double[3];
1962         calibrator.getBias(bias2);
1963         assertArrayEquals(bias1, bias2, 0.0);
1964         final Matrix b1 = calibrator.getBiasAsMatrix();
1965         assertEquals(b1, ba);
1966         final Matrix b2 = new Matrix(3, 1);
1967         calibrator.getBiasAsMatrix(b2);
1968         assertEquals(b1, b2);
1969         final Matrix ma1 = calibrator.getInitialMa();
1970         assertEquals(ma1, new Matrix(3, 3));
1971         final Matrix ma2 = new Matrix(3, 3);
1972         calibrator.getInitialMa(ma2);
1973         assertEquals(ma1, ma2);
1974         assertNull(calibrator.getMeasurements());
1975         assertTrue(calibrator.isCommonAxisUsed());
1976         assertSame(calibrator.getListener(), this);
1977         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
1978         assertFalse(calibrator.isReady());
1979         assertFalse(calibrator.isRunning());
1980         assertNull(calibrator.getEstimatedMa());
1981         assertNull(calibrator.getEstimatedSx());
1982         assertNull(calibrator.getEstimatedSy());
1983         assertNull(calibrator.getEstimatedSz());
1984         assertNull(calibrator.getEstimatedMxy());
1985         assertNull(calibrator.getEstimatedMxz());
1986         assertNull(calibrator.getEstimatedMyx());
1987         assertNull(calibrator.getEstimatedMyz());
1988         assertNull(calibrator.getEstimatedMzx());
1989         assertNull(calibrator.getEstimatedMzy());
1990         assertNull(calibrator.getEstimatedCovariance());
1991         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1992         assertNull(calibrator.getGroundTruthGravityNorm());
1993         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1994         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1995     }
1996 
1997     @Test
1998     public void testConstructor23() throws WrongSizeException {
1999         final Collection<StandardDeviationBodyKinematics> measurements =
2000                 Collections.emptyList();
2001 
2002         final Matrix ba = generateBa();
2003         final double biasX = ba.getElementAtIndex(0);
2004         final double biasY = ba.getElementAtIndex(1);
2005         final double biasZ = ba.getElementAtIndex(2);
2006 
2007         final Acceleration bx = new Acceleration(biasX,
2008                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2009         final Acceleration by = new Acceleration(biasY,
2010                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2011         final Acceleration bz = new Acceleration(biasZ,
2012                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2013 
2014         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2015                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
2016                         true, bx, by, bz);
2017 
2018         // check default values
2019         assertEquals(calibrator.getBiasX(), biasX, 0.0);
2020         assertEquals(calibrator.getBiasY(), biasY, 0.0);
2021         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2022         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2023         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2024         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2025         final Acceleration bx2 = new Acceleration(0.0,
2026                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2027         calibrator.getBiasXAsAcceleration(bx2);
2028         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2029         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2030         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2031         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2032         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2033         final Acceleration by2 = new Acceleration(0.0,
2034                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2035         calibrator.getBiasYAsAcceleration(by2);
2036         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2037         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2038         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2039         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2040         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2041         final Acceleration bz2 = new Acceleration(0.0,
2042                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2043         calibrator.getBiasZAsAcceleration(bz2);
2044         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2045         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2046         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2047         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2048         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2049         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2050         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2051         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2052         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2053         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2054         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2055         final double[] bias1 = calibrator.getBias();
2056         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2057         final double[] bias2 = new double[3];
2058         calibrator.getBias(bias2);
2059         assertArrayEquals(bias1, bias2, 0.0);
2060         final Matrix b1 = calibrator.getBiasAsMatrix();
2061         assertEquals(b1, ba);
2062         final Matrix b2 = new Matrix(3, 1);
2063         calibrator.getBiasAsMatrix(b2);
2064         assertEquals(b1, b2);
2065         final Matrix ma1 = calibrator.getInitialMa();
2066         assertEquals(ma1, new Matrix(3, 3));
2067         final Matrix ma2 = new Matrix(3, 3);
2068         calibrator.getInitialMa(ma2);
2069         assertEquals(ma1, ma2);
2070         assertSame(calibrator.getMeasurements(), measurements);
2071         assertTrue(calibrator.isCommonAxisUsed());
2072         assertNull(calibrator.getListener());
2073         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
2074         assertFalse(calibrator.isReady());
2075         assertFalse(calibrator.isRunning());
2076         assertNull(calibrator.getEstimatedMa());
2077         assertNull(calibrator.getEstimatedSx());
2078         assertNull(calibrator.getEstimatedSy());
2079         assertNull(calibrator.getEstimatedSz());
2080         assertNull(calibrator.getEstimatedMxy());
2081         assertNull(calibrator.getEstimatedMxz());
2082         assertNull(calibrator.getEstimatedMyx());
2083         assertNull(calibrator.getEstimatedMyz());
2084         assertNull(calibrator.getEstimatedMzx());
2085         assertNull(calibrator.getEstimatedMzy());
2086         assertNull(calibrator.getEstimatedCovariance());
2087         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2088         assertNull(calibrator.getGroundTruthGravityNorm());
2089         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2090         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2091     }
2092 
2093     @Test
2094     public void testConstructor24() throws WrongSizeException {
2095         final Collection<StandardDeviationBodyKinematics> measurements =
2096                 Collections.emptyList();
2097 
2098         final Matrix ba = generateBa();
2099         final double biasX = ba.getElementAtIndex(0);
2100         final double biasY = ba.getElementAtIndex(1);
2101         final double biasZ = ba.getElementAtIndex(2);
2102 
2103         final Acceleration bx = new Acceleration(biasX,
2104                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2105         final Acceleration by = new Acceleration(biasY,
2106                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2107         final Acceleration bz = new Acceleration(biasZ,
2108                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2109 
2110         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2111                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
2112                         true, bx, by, bz, this);
2113 
2114         // check default values
2115         assertEquals(calibrator.getBiasX(), biasX, 0.0);
2116         assertEquals(calibrator.getBiasY(), biasY, 0.0);
2117         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2118         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2119         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2120         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2121         final Acceleration bx2 = new Acceleration(0.0,
2122                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2123         calibrator.getBiasXAsAcceleration(bx2);
2124         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2125         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2126         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2127         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2128         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2129         final Acceleration by2 = new Acceleration(0.0,
2130                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2131         calibrator.getBiasYAsAcceleration(by2);
2132         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2133         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2134         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2135         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2136         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2137         final Acceleration bz2 = new Acceleration(0.0,
2138                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2139         calibrator.getBiasZAsAcceleration(bz2);
2140         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2141         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2142         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2143         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2144         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2145         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2146         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2147         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2148         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2149         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2150         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2151         final double[] bias1 = calibrator.getBias();
2152         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2153         final double[] bias2 = new double[3];
2154         calibrator.getBias(bias2);
2155         assertArrayEquals(bias1, bias2, 0.0);
2156         final Matrix b1 = calibrator.getBiasAsMatrix();
2157         assertEquals(b1, ba);
2158         final Matrix b2 = new Matrix(3, 1);
2159         calibrator.getBiasAsMatrix(b2);
2160         assertEquals(b1, b2);
2161         final Matrix ma1 = calibrator.getInitialMa();
2162         assertEquals(ma1, new Matrix(3, 3));
2163         final Matrix ma2 = new Matrix(3, 3);
2164         calibrator.getInitialMa(ma2);
2165         assertEquals(ma1, ma2);
2166         assertSame(calibrator.getMeasurements(), measurements);
2167         assertTrue(calibrator.isCommonAxisUsed());
2168         assertSame(calibrator.getListener(), this);
2169         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
2170         assertFalse(calibrator.isReady());
2171         assertFalse(calibrator.isRunning());
2172         assertNull(calibrator.getEstimatedMa());
2173         assertNull(calibrator.getEstimatedSx());
2174         assertNull(calibrator.getEstimatedSy());
2175         assertNull(calibrator.getEstimatedSz());
2176         assertNull(calibrator.getEstimatedMxy());
2177         assertNull(calibrator.getEstimatedMxz());
2178         assertNull(calibrator.getEstimatedMyx());
2179         assertNull(calibrator.getEstimatedMyz());
2180         assertNull(calibrator.getEstimatedMzx());
2181         assertNull(calibrator.getEstimatedMzy());
2182         assertNull(calibrator.getEstimatedCovariance());
2183         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2184         assertNull(calibrator.getGroundTruthGravityNorm());
2185         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2186         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2187     }
2188 
2189     @Test
2190     public void testConstructor25() throws WrongSizeException {
2191         final Matrix ba = generateBa();
2192         final double biasX = ba.getElementAtIndex(0);
2193         final double biasY = ba.getElementAtIndex(1);
2194         final double biasZ = ba.getElementAtIndex(2);
2195 
2196         final Matrix ma = generateMaCommonAxis();
2197         final double sx = ma.getElementAt(0, 0);
2198         final double sy = ma.getElementAt(1, 1);
2199         final double sz = ma.getElementAt(2, 2);
2200 
2201         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2202                 new KnownBiasAndGravityNormAccelerometerCalibrator(
2203                         biasX, biasY, biasZ,
2204                         sx, sy, sz);
2205 
2206         // check default values
2207         assertEquals(calibrator.getBiasX(), biasX, 0.0);
2208         assertEquals(calibrator.getBiasY(), biasY, 0.0);
2209         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2210         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2211         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2212         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2213         final Acceleration bx2 = new Acceleration(0.0,
2214                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2215         calibrator.getBiasXAsAcceleration(bx2);
2216         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2217         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2218         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2219         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2220         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2221         final Acceleration by2 = new Acceleration(0.0,
2222                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2223         calibrator.getBiasYAsAcceleration(by2);
2224         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2225         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2226         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2227         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2228         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2229         final Acceleration bz2 = new Acceleration(0.0,
2230                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2231         calibrator.getBiasZAsAcceleration(bz2);
2232         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2233         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2234         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2235         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2236         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2237         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2238         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2239         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2240         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2241         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2242         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2243         final double[] bias1 = calibrator.getBias();
2244         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2245         final double[] bias2 = new double[3];
2246         calibrator.getBias(bias2);
2247         assertArrayEquals(bias1, bias2, 0.0);
2248         final Matrix b1 = calibrator.getBiasAsMatrix();
2249         assertEquals(b1, ba);
2250         final Matrix b2 = new Matrix(3, 1);
2251         calibrator.getBiasAsMatrix(b2);
2252         assertEquals(b1, b2);
2253         final Matrix ma1 = calibrator.getInitialMa();
2254         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2255         final Matrix ma2 = new Matrix(3, 3);
2256         calibrator.getInitialMa(ma2);
2257         assertEquals(ma1, ma2);
2258         assertNull(calibrator.getMeasurements());
2259         assertFalse(calibrator.isCommonAxisUsed());
2260         assertNull(calibrator.getListener());
2261         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2262         assertFalse(calibrator.isReady());
2263         assertFalse(calibrator.isRunning());
2264         assertNull(calibrator.getEstimatedMa());
2265         assertNull(calibrator.getEstimatedSx());
2266         assertNull(calibrator.getEstimatedSy());
2267         assertNull(calibrator.getEstimatedSz());
2268         assertNull(calibrator.getEstimatedMxy());
2269         assertNull(calibrator.getEstimatedMxz());
2270         assertNull(calibrator.getEstimatedMyx());
2271         assertNull(calibrator.getEstimatedMyz());
2272         assertNull(calibrator.getEstimatedMzx());
2273         assertNull(calibrator.getEstimatedMzy());
2274         assertNull(calibrator.getEstimatedCovariance());
2275         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2276         assertNull(calibrator.getGroundTruthGravityNorm());
2277         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2278         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2279     }
2280 
2281     @Test
2282     public void testConstructor26() throws WrongSizeException {
2283         final Collection<StandardDeviationBodyKinematics> measurements =
2284                 Collections.emptyList();
2285 
2286         final Matrix ba = generateBa();
2287         final double biasX = ba.getElementAtIndex(0);
2288         final double biasY = ba.getElementAtIndex(1);
2289         final double biasZ = ba.getElementAtIndex(2);
2290 
2291         final Matrix ma = generateMaCommonAxis();
2292         final double sx = ma.getElementAt(0, 0);
2293         final double sy = ma.getElementAt(1, 1);
2294         final double sz = ma.getElementAt(2, 2);
2295 
2296         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2297                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
2298                         biasX, biasY, biasZ, sx, sy, sz);
2299 
2300         // check default values
2301         assertEquals(calibrator.getBiasX(), biasX, 0.0);
2302         assertEquals(calibrator.getBiasY(), biasY, 0.0);
2303         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2304         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2305         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2306         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2307         final Acceleration bx2 = new Acceleration(0.0,
2308                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2309         calibrator.getBiasXAsAcceleration(bx2);
2310         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2311         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2312         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2313         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2314         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2315         final Acceleration by2 = new Acceleration(0.0,
2316                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2317         calibrator.getBiasYAsAcceleration(by2);
2318         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2319         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2320         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2321         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2322         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2323         final Acceleration bz2 = new Acceleration(0.0,
2324                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2325         calibrator.getBiasZAsAcceleration(bz2);
2326         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2327         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2328         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2329         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2330         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2331         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2332         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2333         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2334         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2335         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2336         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2337         final double[] bias1 = calibrator.getBias();
2338         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2339         final double[] bias2 = new double[3];
2340         calibrator.getBias(bias2);
2341         assertArrayEquals(bias1, bias2, 0.0);
2342         final Matrix b1 = calibrator.getBiasAsMatrix();
2343         assertEquals(b1, ba);
2344         final Matrix b2 = new Matrix(3, 1);
2345         calibrator.getBiasAsMatrix(b2);
2346         assertEquals(b1, b2);
2347         final Matrix ma1 = calibrator.getInitialMa();
2348         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2349         final Matrix ma2 = new Matrix(3, 3);
2350         calibrator.getInitialMa(ma2);
2351         assertEquals(ma1, ma2);
2352         assertSame(calibrator.getMeasurements(), measurements);
2353         assertFalse(calibrator.isCommonAxisUsed());
2354         assertNull(calibrator.getListener());
2355         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2356         assertFalse(calibrator.isReady());
2357         assertFalse(calibrator.isRunning());
2358         assertNull(calibrator.getEstimatedMa());
2359         assertNull(calibrator.getEstimatedSx());
2360         assertNull(calibrator.getEstimatedSy());
2361         assertNull(calibrator.getEstimatedSz());
2362         assertNull(calibrator.getEstimatedMxy());
2363         assertNull(calibrator.getEstimatedMxz());
2364         assertNull(calibrator.getEstimatedMyx());
2365         assertNull(calibrator.getEstimatedMyz());
2366         assertNull(calibrator.getEstimatedMzx());
2367         assertNull(calibrator.getEstimatedMzy());
2368         assertNull(calibrator.getEstimatedCovariance());
2369         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2370         assertNull(calibrator.getGroundTruthGravityNorm());
2371         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2372         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2373     }
2374 
2375     @Test
2376     public void testConstructor27() throws WrongSizeException {
2377         final Collection<StandardDeviationBodyKinematics> measurements =
2378                 Collections.emptyList();
2379 
2380         final Matrix ba = generateBa();
2381         final double biasX = ba.getElementAtIndex(0);
2382         final double biasY = ba.getElementAtIndex(1);
2383         final double biasZ = ba.getElementAtIndex(2);
2384 
2385         final Matrix ma = generateMaCommonAxis();
2386         final double sx = ma.getElementAt(0, 0);
2387         final double sy = ma.getElementAt(1, 1);
2388         final double sz = ma.getElementAt(2, 2);
2389 
2390         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2391                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
2392                         biasX, biasY, biasZ, sx, sy, sz, this);
2393 
2394         // check default values
2395         assertEquals(calibrator.getBiasX(), biasX, 0.0);
2396         assertEquals(calibrator.getBiasY(), biasY, 0.0);
2397         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2398         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2399         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2400         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2401         final Acceleration bx2 = new Acceleration(0.0,
2402                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2403         calibrator.getBiasXAsAcceleration(bx2);
2404         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2405         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2406         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2407         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2408         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2409         final Acceleration by2 = new Acceleration(0.0,
2410                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2411         calibrator.getBiasYAsAcceleration(by2);
2412         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2413         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2414         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2415         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2416         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2417         final Acceleration bz2 = new Acceleration(0.0,
2418                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2419         calibrator.getBiasZAsAcceleration(bz2);
2420         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2421         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2422         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2423         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2424         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2425         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2426         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2427         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2428         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2429         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2430         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2431         final double[] bias1 = calibrator.getBias();
2432         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2433         final double[] bias2 = new double[3];
2434         calibrator.getBias(bias2);
2435         assertArrayEquals(bias1, bias2, 0.0);
2436         final Matrix b1 = calibrator.getBiasAsMatrix();
2437         assertEquals(b1, ba);
2438         final Matrix b2 = new Matrix(3, 1);
2439         calibrator.getBiasAsMatrix(b2);
2440         assertEquals(b1, b2);
2441         final Matrix ma1 = calibrator.getInitialMa();
2442         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2443         final Matrix ma2 = new Matrix(3, 3);
2444         calibrator.getInitialMa(ma2);
2445         assertEquals(ma1, ma2);
2446         assertSame(calibrator.getMeasurements(), measurements);
2447         assertFalse(calibrator.isCommonAxisUsed());
2448         assertSame(calibrator.getListener(), this);
2449         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2450         assertFalse(calibrator.isReady());
2451         assertFalse(calibrator.isRunning());
2452         assertNull(calibrator.getEstimatedMa());
2453         assertNull(calibrator.getEstimatedSx());
2454         assertNull(calibrator.getEstimatedSy());
2455         assertNull(calibrator.getEstimatedSz());
2456         assertNull(calibrator.getEstimatedMxy());
2457         assertNull(calibrator.getEstimatedMxz());
2458         assertNull(calibrator.getEstimatedMyx());
2459         assertNull(calibrator.getEstimatedMyz());
2460         assertNull(calibrator.getEstimatedMzx());
2461         assertNull(calibrator.getEstimatedMzy());
2462         assertNull(calibrator.getEstimatedCovariance());
2463         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2464         assertNull(calibrator.getGroundTruthGravityNorm());
2465         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2466         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2467     }
2468 
2469     @Test
2470     public void testConstructor28() throws WrongSizeException {
2471         final Matrix ba = generateBa();
2472         final double biasX = ba.getElementAtIndex(0);
2473         final double biasY = ba.getElementAtIndex(1);
2474         final double biasZ = ba.getElementAtIndex(2);
2475 
2476         final Matrix ma = generateMaCommonAxis();
2477         final double sx = ma.getElementAt(0, 0);
2478         final double sy = ma.getElementAt(1, 1);
2479         final double sz = ma.getElementAt(2, 2);
2480 
2481         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2482                 new KnownBiasAndGravityNormAccelerometerCalibrator(
2483                         true, biasX, biasY, biasZ, sx, sy, sz);
2484 
2485         // check default values
2486         assertEquals(calibrator.getBiasX(), biasX, 0.0);
2487         assertEquals(calibrator.getBiasY(), biasY, 0.0);
2488         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2489         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2490         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2491         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2492         final Acceleration bx2 = new Acceleration(0.0,
2493                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2494         calibrator.getBiasXAsAcceleration(bx2);
2495         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2496         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2497         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2498         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2499         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2500         final Acceleration by2 = new Acceleration(0.0,
2501                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2502         calibrator.getBiasYAsAcceleration(by2);
2503         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2504         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2505         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2506         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2507         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2508         final Acceleration bz2 = new Acceleration(0.0,
2509                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2510         calibrator.getBiasZAsAcceleration(bz2);
2511         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2512         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2513         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2514         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2515         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2516         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2517         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2518         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2519         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2520         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2521         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2522         final double[] bias1 = calibrator.getBias();
2523         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2524         final double[] bias2 = new double[3];
2525         calibrator.getBias(bias2);
2526         assertArrayEquals(bias1, bias2, 0.0);
2527         final Matrix b1 = calibrator.getBiasAsMatrix();
2528         assertEquals(b1, ba);
2529         final Matrix b2 = new Matrix(3, 1);
2530         calibrator.getBiasAsMatrix(b2);
2531         assertEquals(b1, b2);
2532         final Matrix ma1 = calibrator.getInitialMa();
2533         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2534         final Matrix ma2 = new Matrix(3, 3);
2535         calibrator.getInitialMa(ma2);
2536         assertEquals(ma1, ma2);
2537         assertNull(calibrator.getMeasurements());
2538         assertTrue(calibrator.isCommonAxisUsed());
2539         assertNull(calibrator.getListener());
2540         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
2541         assertFalse(calibrator.isReady());
2542         assertFalse(calibrator.isRunning());
2543         assertNull(calibrator.getEstimatedMa());
2544         assertNull(calibrator.getEstimatedSx());
2545         assertNull(calibrator.getEstimatedSy());
2546         assertNull(calibrator.getEstimatedSz());
2547         assertNull(calibrator.getEstimatedMxy());
2548         assertNull(calibrator.getEstimatedMxz());
2549         assertNull(calibrator.getEstimatedMyx());
2550         assertNull(calibrator.getEstimatedMyz());
2551         assertNull(calibrator.getEstimatedMzx());
2552         assertNull(calibrator.getEstimatedMzy());
2553         assertNull(calibrator.getEstimatedCovariance());
2554         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2555         assertNull(calibrator.getGroundTruthGravityNorm());
2556         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2557         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2558     }
2559 
2560     @Test
2561     public void testConstructor29() throws WrongSizeException {
2562         final Matrix ba = generateBa();
2563         final double biasX = ba.getElementAtIndex(0);
2564         final double biasY = ba.getElementAtIndex(1);
2565         final double biasZ = ba.getElementAtIndex(2);
2566 
2567         final Matrix ma = generateMaCommonAxis();
2568         final double sx = ma.getElementAt(0, 0);
2569         final double sy = ma.getElementAt(1, 1);
2570         final double sz = ma.getElementAt(2, 2);
2571 
2572         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2573                 new KnownBiasAndGravityNormAccelerometerCalibrator(
2574                         true, biasX, biasY, biasZ,
2575                         sx, sy, sz, this);
2576 
2577         // check default values
2578         assertEquals(calibrator.getBiasX(), biasX, 0.0);
2579         assertEquals(calibrator.getBiasY(), biasY, 0.0);
2580         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2581         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2582         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2583         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2584         final Acceleration bx2 = new Acceleration(0.0,
2585                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2586         calibrator.getBiasXAsAcceleration(bx2);
2587         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2588         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2589         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2590         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2591         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2592         final Acceleration by2 = new Acceleration(0.0,
2593                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2594         calibrator.getBiasYAsAcceleration(by2);
2595         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2596         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2597         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2598         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2599         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2600         final Acceleration bz2 = new Acceleration(0.0,
2601                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2602         calibrator.getBiasZAsAcceleration(bz2);
2603         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2604         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2605         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2606         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2607         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2608         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2609         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2610         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2611         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2612         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2613         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2614         final double[] bias1 = calibrator.getBias();
2615         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2616         final double[] bias2 = new double[3];
2617         calibrator.getBias(bias2);
2618         assertArrayEquals(bias1, bias2, 0.0);
2619         final Matrix b1 = calibrator.getBiasAsMatrix();
2620         assertEquals(b1, ba);
2621         final Matrix b2 = new Matrix(3, 1);
2622         calibrator.getBiasAsMatrix(b2);
2623         assertEquals(b1, b2);
2624         final Matrix ma1 = calibrator.getInitialMa();
2625         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2626         final Matrix ma2 = new Matrix(3, 3);
2627         calibrator.getInitialMa(ma2);
2628         assertEquals(ma1, ma2);
2629         assertNull(calibrator.getMeasurements());
2630         assertTrue(calibrator.isCommonAxisUsed());
2631         assertSame(calibrator.getListener(), this);
2632         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
2633         assertFalse(calibrator.isReady());
2634         assertFalse(calibrator.isRunning());
2635         assertNull(calibrator.getEstimatedMa());
2636         assertNull(calibrator.getEstimatedSx());
2637         assertNull(calibrator.getEstimatedSy());
2638         assertNull(calibrator.getEstimatedSz());
2639         assertNull(calibrator.getEstimatedMxy());
2640         assertNull(calibrator.getEstimatedMxz());
2641         assertNull(calibrator.getEstimatedMyx());
2642         assertNull(calibrator.getEstimatedMyz());
2643         assertNull(calibrator.getEstimatedMzx());
2644         assertNull(calibrator.getEstimatedMzy());
2645         assertNull(calibrator.getEstimatedCovariance());
2646         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2647         assertNull(calibrator.getGroundTruthGravityNorm());
2648         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2649         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2650     }
2651 
2652     @Test
2653     public void testConstructor30() throws WrongSizeException {
2654         final Collection<StandardDeviationBodyKinematics> measurements =
2655                 Collections.emptyList();
2656         final Matrix ba = generateBa();
2657         final double biasX = ba.getElementAtIndex(0);
2658         final double biasY = ba.getElementAtIndex(1);
2659         final double biasZ = ba.getElementAtIndex(2);
2660 
2661         final Matrix ma = generateMaCommonAxis();
2662         final double sx = ma.getElementAt(0, 0);
2663         final double sy = ma.getElementAt(1, 1);
2664         final double sz = ma.getElementAt(2, 2);
2665 
2666         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2667                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
2668                         true, biasX, biasY, biasZ,
2669                         sx, sy, sz);
2670 
2671         // check default values
2672         assertEquals(calibrator.getBiasX(), biasX, 0.0);
2673         assertEquals(calibrator.getBiasY(), biasY, 0.0);
2674         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2675         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2676         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2677         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2678         final Acceleration bx2 = new Acceleration(0.0,
2679                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2680         calibrator.getBiasXAsAcceleration(bx2);
2681         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2682         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2683         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2684         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2685         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2686         final Acceleration by2 = new Acceleration(0.0,
2687                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2688         calibrator.getBiasYAsAcceleration(by2);
2689         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2690         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2691         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2692         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2693         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2694         final Acceleration bz2 = new Acceleration(0.0,
2695                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2696         calibrator.getBiasZAsAcceleration(bz2);
2697         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2698         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2699         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2700         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2701         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2702         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2703         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2704         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2705         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2706         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2707         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2708         final double[] bias1 = calibrator.getBias();
2709         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2710         final double[] bias2 = new double[3];
2711         calibrator.getBias(bias2);
2712         assertArrayEquals(bias1, bias2, 0.0);
2713         final Matrix b1 = calibrator.getBiasAsMatrix();
2714         assertEquals(b1, ba);
2715         final Matrix b2 = new Matrix(3, 1);
2716         calibrator.getBiasAsMatrix(b2);
2717         assertEquals(b1, b2);
2718         final Matrix ma1 = calibrator.getInitialMa();
2719         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2720         final Matrix ma2 = new Matrix(3, 3);
2721         calibrator.getInitialMa(ma2);
2722         assertEquals(ma1, ma2);
2723         assertSame(calibrator.getMeasurements(), measurements);
2724         assertTrue(calibrator.isCommonAxisUsed());
2725         assertNull(calibrator.getListener());
2726         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
2727         assertFalse(calibrator.isReady());
2728         assertFalse(calibrator.isRunning());
2729         assertNull(calibrator.getEstimatedMa());
2730         assertNull(calibrator.getEstimatedSx());
2731         assertNull(calibrator.getEstimatedSy());
2732         assertNull(calibrator.getEstimatedSz());
2733         assertNull(calibrator.getEstimatedMxy());
2734         assertNull(calibrator.getEstimatedMxz());
2735         assertNull(calibrator.getEstimatedMyx());
2736         assertNull(calibrator.getEstimatedMyz());
2737         assertNull(calibrator.getEstimatedMzx());
2738         assertNull(calibrator.getEstimatedMzy());
2739         assertNull(calibrator.getEstimatedCovariance());
2740         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2741         assertNull(calibrator.getGroundTruthGravityNorm());
2742         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2743         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2744     }
2745 
2746     @Test
2747     public void testConstructor31() throws WrongSizeException {
2748         final Collection<StandardDeviationBodyKinematics> measurements =
2749                 Collections.emptyList();
2750         final Matrix ba = generateBa();
2751         final double biasX = ba.getElementAtIndex(0);
2752         final double biasY = ba.getElementAtIndex(1);
2753         final double biasZ = ba.getElementAtIndex(2);
2754 
2755         final Matrix ma = generateMaCommonAxis();
2756         final double sx = ma.getElementAt(0, 0);
2757         final double sy = ma.getElementAt(1, 1);
2758         final double sz = ma.getElementAt(2, 2);
2759 
2760         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2761                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
2762                         true, biasX, biasY, biasZ,
2763                         sx, sy, sz, this);
2764 
2765         // check default values
2766         assertEquals(calibrator.getBiasX(), biasX, 0.0);
2767         assertEquals(calibrator.getBiasY(), biasY, 0.0);
2768         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2769         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2770         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2771         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2772         final Acceleration bx2 = new Acceleration(0.0,
2773                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2774         calibrator.getBiasXAsAcceleration(bx2);
2775         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2776         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2777         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2778         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2779         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2780         final Acceleration by2 = new Acceleration(0.0,
2781                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2782         calibrator.getBiasYAsAcceleration(by2);
2783         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2784         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2785         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2786         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2787         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2788         final Acceleration bz2 = new Acceleration(0.0,
2789                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2790         calibrator.getBiasZAsAcceleration(bz2);
2791         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2792         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2793         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2794         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2795         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2796         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2797         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2798         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2799         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2800         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2801         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2802         final double[] bias1 = calibrator.getBias();
2803         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2804         final double[] bias2 = new double[3];
2805         calibrator.getBias(bias2);
2806         assertArrayEquals(bias1, bias2, 0.0);
2807         final Matrix b1 = calibrator.getBiasAsMatrix();
2808         assertEquals(b1, ba);
2809         final Matrix b2 = new Matrix(3, 1);
2810         calibrator.getBiasAsMatrix(b2);
2811         assertEquals(b1, b2);
2812         final Matrix ma1 = calibrator.getInitialMa();
2813         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2814         final Matrix ma2 = new Matrix(3, 3);
2815         calibrator.getInitialMa(ma2);
2816         assertEquals(ma1, ma2);
2817         assertSame(calibrator.getMeasurements(), measurements);
2818         assertTrue(calibrator.isCommonAxisUsed());
2819         assertSame(calibrator.getListener(), this);
2820         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
2821         assertFalse(calibrator.isReady());
2822         assertFalse(calibrator.isRunning());
2823         assertNull(calibrator.getEstimatedMa());
2824         assertNull(calibrator.getEstimatedSx());
2825         assertNull(calibrator.getEstimatedSy());
2826         assertNull(calibrator.getEstimatedSz());
2827         assertNull(calibrator.getEstimatedMxy());
2828         assertNull(calibrator.getEstimatedMxz());
2829         assertNull(calibrator.getEstimatedMyx());
2830         assertNull(calibrator.getEstimatedMyz());
2831         assertNull(calibrator.getEstimatedMzx());
2832         assertNull(calibrator.getEstimatedMzy());
2833         assertNull(calibrator.getEstimatedCovariance());
2834         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2835         assertNull(calibrator.getGroundTruthGravityNorm());
2836         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2837         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2838     }
2839 
2840     @Test
2841     public void testConstructor32() throws WrongSizeException {
2842         final Matrix ba = generateBa();
2843         final double biasX = ba.getElementAtIndex(0);
2844         final double biasY = ba.getElementAtIndex(1);
2845         final double biasZ = ba.getElementAtIndex(2);
2846 
2847         final Matrix ma = generateMaCommonAxis();
2848         final double sx = ma.getElementAt(0, 0);
2849         final double sy = ma.getElementAt(1, 1);
2850         final double sz = ma.getElementAt(2, 2);
2851 
2852         final Acceleration bx = new Acceleration(biasX,
2853                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2854         final Acceleration by = new Acceleration(biasY,
2855                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2856         final Acceleration bz = new Acceleration(biasZ,
2857                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2858 
2859         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2860                 new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz,
2861                         sx, sy, sz);
2862 
2863         // check default values
2864         assertEquals(calibrator.getBiasX(), biasX, 0.0);
2865         assertEquals(calibrator.getBiasY(), biasY, 0.0);
2866         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2867         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2868         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2869         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2870         final Acceleration bx2 = new Acceleration(0.0,
2871                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2872         calibrator.getBiasXAsAcceleration(bx2);
2873         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2874         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2875         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2876         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2877         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2878         final Acceleration by2 = new Acceleration(0.0,
2879                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2880         calibrator.getBiasYAsAcceleration(by2);
2881         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2882         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2883         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2884         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2885         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2886         final Acceleration bz2 = new Acceleration(0.0,
2887                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2888         calibrator.getBiasZAsAcceleration(bz2);
2889         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2890         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2891         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2892         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2893         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2894         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2895         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2896         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2897         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2898         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2899         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2900         final double[] bias1 = calibrator.getBias();
2901         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2902         final double[] bias2 = new double[3];
2903         calibrator.getBias(bias2);
2904         assertArrayEquals(bias1, bias2, 0.0);
2905         final Matrix b1 = calibrator.getBiasAsMatrix();
2906         assertEquals(b1, ba);
2907         final Matrix b2 = new Matrix(3, 1);
2908         calibrator.getBiasAsMatrix(b2);
2909         assertEquals(b1, b2);
2910         final Matrix ma1 = calibrator.getInitialMa();
2911         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2912         final Matrix ma2 = new Matrix(3, 3);
2913         calibrator.getInitialMa(ma2);
2914         assertEquals(ma1, ma2);
2915         assertNull(calibrator.getMeasurements());
2916         assertFalse(calibrator.isCommonAxisUsed());
2917         assertNull(calibrator.getListener());
2918         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2919         assertFalse(calibrator.isReady());
2920         assertFalse(calibrator.isRunning());
2921         assertNull(calibrator.getEstimatedMa());
2922         assertNull(calibrator.getEstimatedSx());
2923         assertNull(calibrator.getEstimatedSy());
2924         assertNull(calibrator.getEstimatedSz());
2925         assertNull(calibrator.getEstimatedMxy());
2926         assertNull(calibrator.getEstimatedMxz());
2927         assertNull(calibrator.getEstimatedMyx());
2928         assertNull(calibrator.getEstimatedMyz());
2929         assertNull(calibrator.getEstimatedMzx());
2930         assertNull(calibrator.getEstimatedMzy());
2931         assertNull(calibrator.getEstimatedCovariance());
2932         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2933         assertNull(calibrator.getGroundTruthGravityNorm());
2934         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2935         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2936     }
2937 
2938     @Test
2939     public void testConstructor33() throws WrongSizeException {
2940         final Matrix ba = generateBa();
2941         final double biasX = ba.getElementAtIndex(0);
2942         final double biasY = ba.getElementAtIndex(1);
2943         final double biasZ = ba.getElementAtIndex(2);
2944 
2945         final Matrix ma = generateMaCommonAxis();
2946         final double sx = ma.getElementAt(0, 0);
2947         final double sy = ma.getElementAt(1, 1);
2948         final double sz = ma.getElementAt(2, 2);
2949 
2950         final Acceleration bx = new Acceleration(biasX,
2951                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2952         final Acceleration by = new Acceleration(biasY,
2953                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2954         final Acceleration bz = new Acceleration(biasZ,
2955                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2956 
2957         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
2958                 new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz,
2959                         sx, sy, sz, this);
2960 
2961         // check default values
2962         assertEquals(calibrator.getBiasX(), biasX, 0.0);
2963         assertEquals(calibrator.getBiasY(), biasY, 0.0);
2964         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2965         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
2966         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2967         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2968         final Acceleration bx2 = new Acceleration(0.0,
2969                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2970         calibrator.getBiasXAsAcceleration(bx2);
2971         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2972         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2973         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
2974         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2975         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2976         final Acceleration by2 = new Acceleration(0.0,
2977                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2978         calibrator.getBiasYAsAcceleration(by2);
2979         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2980         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2981         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
2982         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2983         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2984         final Acceleration bz2 = new Acceleration(0.0,
2985                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2986         calibrator.getBiasZAsAcceleration(bz2);
2987         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2988         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2989         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2990         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2991         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2992         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2993         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2994         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2995         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2996         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2997         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2998         final double[] bias1 = calibrator.getBias();
2999         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3000         final double[] bias2 = new double[3];
3001         calibrator.getBias(bias2);
3002         assertArrayEquals(bias1, bias2, 0.0);
3003         final Matrix b1 = calibrator.getBiasAsMatrix();
3004         assertEquals(b1, ba);
3005         final Matrix b2 = new Matrix(3, 1);
3006         calibrator.getBiasAsMatrix(b2);
3007         assertEquals(b1, b2);
3008         final Matrix ma1 = calibrator.getInitialMa();
3009         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3010         final Matrix ma2 = new Matrix(3, 3);
3011         calibrator.getInitialMa(ma2);
3012         assertEquals(ma1, ma2);
3013         assertNull(calibrator.getMeasurements());
3014         assertFalse(calibrator.isCommonAxisUsed());
3015         assertSame(calibrator.getListener(), this);
3016         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3017         assertFalse(calibrator.isReady());
3018         assertFalse(calibrator.isRunning());
3019         assertNull(calibrator.getEstimatedMa());
3020         assertNull(calibrator.getEstimatedSx());
3021         assertNull(calibrator.getEstimatedSy());
3022         assertNull(calibrator.getEstimatedSz());
3023         assertNull(calibrator.getEstimatedMxy());
3024         assertNull(calibrator.getEstimatedMxz());
3025         assertNull(calibrator.getEstimatedMyx());
3026         assertNull(calibrator.getEstimatedMyz());
3027         assertNull(calibrator.getEstimatedMzx());
3028         assertNull(calibrator.getEstimatedMzy());
3029         assertNull(calibrator.getEstimatedCovariance());
3030         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3031         assertNull(calibrator.getGroundTruthGravityNorm());
3032         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3033         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3034     }
3035 
3036     @Test
3037     public void testConstructor34() throws WrongSizeException {
3038         final Collection<StandardDeviationBodyKinematics> measurements =
3039                 Collections.emptyList();
3040 
3041         final Matrix ba = generateBa();
3042         final double biasX = ba.getElementAtIndex(0);
3043         final double biasY = ba.getElementAtIndex(1);
3044         final double biasZ = ba.getElementAtIndex(2);
3045 
3046         final Matrix ma = generateMaCommonAxis();
3047         final double sx = ma.getElementAt(0, 0);
3048         final double sy = ma.getElementAt(1, 1);
3049         final double sz = ma.getElementAt(2, 2);
3050 
3051         final Acceleration bx = new Acceleration(biasX,
3052                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3053         final Acceleration by = new Acceleration(biasY,
3054                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3055         final Acceleration bz = new Acceleration(biasZ,
3056                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3057 
3058         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3059                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
3060                         bx, by, bz, sx, sy, sz);
3061 
3062         // check default values
3063         assertEquals(calibrator.getBiasX(), biasX, 0.0);
3064         assertEquals(calibrator.getBiasY(), biasY, 0.0);
3065         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3066         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3067         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3068         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3069         final Acceleration bx2 = new Acceleration(0.0,
3070                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3071         calibrator.getBiasXAsAcceleration(bx2);
3072         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3073         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3074         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3075         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3076         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3077         final Acceleration by2 = new Acceleration(0.0,
3078                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3079         calibrator.getBiasYAsAcceleration(by2);
3080         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3081         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3082         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3083         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3084         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3085         final Acceleration bz2 = new Acceleration(0.0,
3086                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3087         calibrator.getBiasZAsAcceleration(bz2);
3088         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3089         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3090         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3091         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3092         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3093         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3094         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3095         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3096         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3097         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3098         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3099         final double[] bias1 = calibrator.getBias();
3100         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3101         final double[] bias2 = new double[3];
3102         calibrator.getBias(bias2);
3103         assertArrayEquals(bias1, bias2, 0.0);
3104         final Matrix b1 = calibrator.getBiasAsMatrix();
3105         assertEquals(b1, ba);
3106         final Matrix b2 = new Matrix(3, 1);
3107         calibrator.getBiasAsMatrix(b2);
3108         assertEquals(b1, b2);
3109         final Matrix ma1 = calibrator.getInitialMa();
3110         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3111         final Matrix ma2 = new Matrix(3, 3);
3112         calibrator.getInitialMa(ma2);
3113         assertEquals(ma1, ma2);
3114         assertSame(calibrator.getMeasurements(), measurements);
3115         assertFalse(calibrator.isCommonAxisUsed());
3116         assertNull(calibrator.getListener());
3117         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3118         assertFalse(calibrator.isReady());
3119         assertFalse(calibrator.isRunning());
3120         assertNull(calibrator.getEstimatedMa());
3121         assertNull(calibrator.getEstimatedSx());
3122         assertNull(calibrator.getEstimatedSy());
3123         assertNull(calibrator.getEstimatedSz());
3124         assertNull(calibrator.getEstimatedMxy());
3125         assertNull(calibrator.getEstimatedMxz());
3126         assertNull(calibrator.getEstimatedMyx());
3127         assertNull(calibrator.getEstimatedMyz());
3128         assertNull(calibrator.getEstimatedMzx());
3129         assertNull(calibrator.getEstimatedMzy());
3130         assertNull(calibrator.getEstimatedCovariance());
3131         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3132         assertNull(calibrator.getGroundTruthGravityNorm());
3133         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3134         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3135     }
3136 
3137     @Test
3138     public void testConstructor35() throws WrongSizeException {
3139         final Collection<StandardDeviationBodyKinematics> measurements =
3140                 Collections.emptyList();
3141 
3142         final Matrix ba = generateBa();
3143         final double biasX = ba.getElementAtIndex(0);
3144         final double biasY = ba.getElementAtIndex(1);
3145         final double biasZ = ba.getElementAtIndex(2);
3146 
3147         final Matrix ma = generateMaCommonAxis();
3148         final double sx = ma.getElementAt(0, 0);
3149         final double sy = ma.getElementAt(1, 1);
3150         final double sz = ma.getElementAt(2, 2);
3151 
3152         final Acceleration bx = new Acceleration(biasX,
3153                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3154         final Acceleration by = new Acceleration(biasY,
3155                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3156         final Acceleration bz = new Acceleration(biasZ,
3157                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3158 
3159         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3160                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
3161                         bx, by, bz, sx, sy, sz, this);
3162 
3163         // check default values
3164         assertEquals(calibrator.getBiasX(), biasX, 0.0);
3165         assertEquals(calibrator.getBiasY(), biasY, 0.0);
3166         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3167         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3168         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3169         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3170         final Acceleration bx2 = new Acceleration(0.0,
3171                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3172         calibrator.getBiasXAsAcceleration(bx2);
3173         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3174         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3175         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3176         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3177         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3178         final Acceleration by2 = new Acceleration(0.0,
3179                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3180         calibrator.getBiasYAsAcceleration(by2);
3181         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3182         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3183         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3184         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3185         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3186         final Acceleration bz2 = new Acceleration(0.0,
3187                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3188         calibrator.getBiasZAsAcceleration(bz2);
3189         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3190         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3191         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3192         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3193         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3194         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3195         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3196         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3197         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3198         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3199         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3200         final double[] bias1 = calibrator.getBias();
3201         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3202         final double[] bias2 = new double[3];
3203         calibrator.getBias(bias2);
3204         assertArrayEquals(bias1, bias2, 0.0);
3205         final Matrix b1 = calibrator.getBiasAsMatrix();
3206         assertEquals(b1, ba);
3207         final Matrix b2 = new Matrix(3, 1);
3208         calibrator.getBiasAsMatrix(b2);
3209         assertEquals(b1, b2);
3210         final Matrix ma1 = calibrator.getInitialMa();
3211         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3212         final Matrix ma2 = new Matrix(3, 3);
3213         calibrator.getInitialMa(ma2);
3214         assertEquals(ma1, ma2);
3215         assertSame(calibrator.getMeasurements(), measurements);
3216         assertFalse(calibrator.isCommonAxisUsed());
3217         assertSame(calibrator.getListener(), this);
3218         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3219         assertFalse(calibrator.isReady());
3220         assertFalse(calibrator.isRunning());
3221         assertNull(calibrator.getEstimatedMa());
3222         assertNull(calibrator.getEstimatedSx());
3223         assertNull(calibrator.getEstimatedSy());
3224         assertNull(calibrator.getEstimatedSz());
3225         assertNull(calibrator.getEstimatedMxy());
3226         assertNull(calibrator.getEstimatedMxz());
3227         assertNull(calibrator.getEstimatedMyx());
3228         assertNull(calibrator.getEstimatedMyz());
3229         assertNull(calibrator.getEstimatedMzx());
3230         assertNull(calibrator.getEstimatedMzy());
3231         assertNull(calibrator.getEstimatedCovariance());
3232         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3233         assertNull(calibrator.getGroundTruthGravityNorm());
3234         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3235         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3236     }
3237 
3238     @Test
3239     public void testConstructor36() throws WrongSizeException {
3240         final Matrix ba = generateBa();
3241         final double biasX = ba.getElementAtIndex(0);
3242         final double biasY = ba.getElementAtIndex(1);
3243         final double biasZ = ba.getElementAtIndex(2);
3244 
3245         final Matrix ma = generateMaCommonAxis();
3246         final double sx = ma.getElementAt(0, 0);
3247         final double sy = ma.getElementAt(1, 1);
3248         final double sz = ma.getElementAt(2, 2);
3249 
3250         final Acceleration bx = new Acceleration(biasX,
3251                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3252         final Acceleration by = new Acceleration(biasY,
3253                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3254         final Acceleration bz = new Acceleration(biasZ,
3255                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3256 
3257         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3258                 new KnownBiasAndGravityNormAccelerometerCalibrator(true,
3259                         bx, by, bz, sx, sy, sz);
3260 
3261         // check default values
3262         assertEquals(calibrator.getBiasX(), biasX, 0.0);
3263         assertEquals(calibrator.getBiasY(), biasY, 0.0);
3264         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3265         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3266         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3267         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3268         final Acceleration bx2 = new Acceleration(0.0,
3269                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3270         calibrator.getBiasXAsAcceleration(bx2);
3271         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3272         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3273         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3274         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3275         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3276         final Acceleration by2 = new Acceleration(0.0,
3277                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3278         calibrator.getBiasYAsAcceleration(by2);
3279         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3280         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3281         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3282         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3283         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3284         final Acceleration bz2 = new Acceleration(0.0,
3285                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3286         calibrator.getBiasZAsAcceleration(bz2);
3287         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3288         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3289         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3290         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3291         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3292         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3293         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3294         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3295         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3296         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3297         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3298         final double[] bias1 = calibrator.getBias();
3299         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3300         final double[] bias2 = new double[3];
3301         calibrator.getBias(bias2);
3302         assertArrayEquals(bias1, bias2, 0.0);
3303         final Matrix b1 = calibrator.getBiasAsMatrix();
3304         assertEquals(b1, ba);
3305         final Matrix b2 = new Matrix(3, 1);
3306         calibrator.getBiasAsMatrix(b2);
3307         assertEquals(b1, b2);
3308         final Matrix ma1 = calibrator.getInitialMa();
3309         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3310         final Matrix ma2 = new Matrix(3, 3);
3311         calibrator.getInitialMa(ma2);
3312         assertEquals(ma1, ma2);
3313         assertNull(calibrator.getMeasurements());
3314         assertTrue(calibrator.isCommonAxisUsed());
3315         assertNull(calibrator.getListener());
3316         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
3317         assertFalse(calibrator.isReady());
3318         assertFalse(calibrator.isRunning());
3319         assertNull(calibrator.getEstimatedMa());
3320         assertNull(calibrator.getEstimatedSx());
3321         assertNull(calibrator.getEstimatedSy());
3322         assertNull(calibrator.getEstimatedSz());
3323         assertNull(calibrator.getEstimatedMxy());
3324         assertNull(calibrator.getEstimatedMxz());
3325         assertNull(calibrator.getEstimatedMyx());
3326         assertNull(calibrator.getEstimatedMyz());
3327         assertNull(calibrator.getEstimatedMzx());
3328         assertNull(calibrator.getEstimatedMzy());
3329         assertNull(calibrator.getEstimatedCovariance());
3330         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3331         assertNull(calibrator.getGroundTruthGravityNorm());
3332         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3333         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3334     }
3335 
3336     @Test
3337     public void testConstructor37() throws WrongSizeException {
3338         final Matrix ba = generateBa();
3339         final double biasX = ba.getElementAtIndex(0);
3340         final double biasY = ba.getElementAtIndex(1);
3341         final double biasZ = ba.getElementAtIndex(2);
3342 
3343         final Matrix ma = generateMaCommonAxis();
3344         final double sx = ma.getElementAt(0, 0);
3345         final double sy = ma.getElementAt(1, 1);
3346         final double sz = ma.getElementAt(2, 2);
3347 
3348         final Acceleration bx = new Acceleration(biasX,
3349                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3350         final Acceleration by = new Acceleration(biasY,
3351                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3352         final Acceleration bz = new Acceleration(biasZ,
3353                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3354 
3355         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3356                 new KnownBiasAndGravityNormAccelerometerCalibrator(true,
3357                         bx, by, bz, sx, sy, sz, this);
3358 
3359         // check default values
3360         assertEquals(calibrator.getBiasX(), biasX, 0.0);
3361         assertEquals(calibrator.getBiasY(), biasY, 0.0);
3362         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3363         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3364         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3365         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3366         final Acceleration bx2 = new Acceleration(0.0,
3367                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3368         calibrator.getBiasXAsAcceleration(bx2);
3369         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3370         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3371         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3372         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3373         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3374         final Acceleration by2 = new Acceleration(0.0,
3375                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3376         calibrator.getBiasYAsAcceleration(by2);
3377         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3378         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3379         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3380         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3381         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3382         final Acceleration bz2 = new Acceleration(0.0,
3383                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3384         calibrator.getBiasZAsAcceleration(bz2);
3385         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3386         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3387         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3388         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3389         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3390         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3391         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3392         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3393         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3394         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3395         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3396         final double[] bias1 = calibrator.getBias();
3397         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3398         final double[] bias2 = new double[3];
3399         calibrator.getBias(bias2);
3400         assertArrayEquals(bias1, bias2, 0.0);
3401         final Matrix b1 = calibrator.getBiasAsMatrix();
3402         assertEquals(b1, ba);
3403         final Matrix b2 = new Matrix(3, 1);
3404         calibrator.getBiasAsMatrix(b2);
3405         assertEquals(b1, b2);
3406         final Matrix ma1 = calibrator.getInitialMa();
3407         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3408         final Matrix ma2 = new Matrix(3, 3);
3409         calibrator.getInitialMa(ma2);
3410         assertEquals(ma1, ma2);
3411         assertNull(calibrator.getMeasurements());
3412         assertTrue(calibrator.isCommonAxisUsed());
3413         assertSame(calibrator.getListener(), this);
3414         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
3415         assertFalse(calibrator.isReady());
3416         assertFalse(calibrator.isRunning());
3417         assertNull(calibrator.getEstimatedMa());
3418         assertNull(calibrator.getEstimatedSx());
3419         assertNull(calibrator.getEstimatedSy());
3420         assertNull(calibrator.getEstimatedSz());
3421         assertNull(calibrator.getEstimatedMxy());
3422         assertNull(calibrator.getEstimatedMxz());
3423         assertNull(calibrator.getEstimatedMyx());
3424         assertNull(calibrator.getEstimatedMyz());
3425         assertNull(calibrator.getEstimatedMzx());
3426         assertNull(calibrator.getEstimatedMzy());
3427         assertNull(calibrator.getEstimatedCovariance());
3428         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3429         assertNull(calibrator.getGroundTruthGravityNorm());
3430         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3431         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3432     }
3433 
3434     @Test
3435     public void testConstructor38() throws WrongSizeException {
3436         final Collection<StandardDeviationBodyKinematics> measurements =
3437                 Collections.emptyList();
3438 
3439         final Matrix ba = generateBa();
3440         final double biasX = ba.getElementAtIndex(0);
3441         final double biasY = ba.getElementAtIndex(1);
3442         final double biasZ = ba.getElementAtIndex(2);
3443 
3444         final Matrix ma = generateMaCommonAxis();
3445         final double sx = ma.getElementAt(0, 0);
3446         final double sy = ma.getElementAt(1, 1);
3447         final double sz = ma.getElementAt(2, 2);
3448 
3449         final Acceleration bx = new Acceleration(biasX,
3450                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3451         final Acceleration by = new Acceleration(biasY,
3452                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3453         final Acceleration bz = new Acceleration(biasZ,
3454                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3455 
3456         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3457                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
3458                         true, bx, by, bz, sx, sy, sz);
3459 
3460         // check default values
3461         assertEquals(calibrator.getBiasX(), biasX, 0.0);
3462         assertEquals(calibrator.getBiasY(), biasY, 0.0);
3463         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3464         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3465         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3466         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3467         final Acceleration bx2 = new Acceleration(0.0,
3468                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3469         calibrator.getBiasXAsAcceleration(bx2);
3470         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3471         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3472         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3473         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3474         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3475         final Acceleration by2 = new Acceleration(0.0,
3476                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3477         calibrator.getBiasYAsAcceleration(by2);
3478         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3479         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3480         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3481         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3482         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3483         final Acceleration bz2 = new Acceleration(0.0,
3484                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3485         calibrator.getBiasZAsAcceleration(bz2);
3486         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3487         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3488         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3489         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3490         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3491         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3492         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3493         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3494         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3495         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3496         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3497         final double[] bias1 = calibrator.getBias();
3498         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3499         final double[] bias2 = new double[3];
3500         calibrator.getBias(bias2);
3501         assertArrayEquals(bias1, bias2, 0.0);
3502         final Matrix b1 = calibrator.getBiasAsMatrix();
3503         assertEquals(b1, ba);
3504         final Matrix b2 = new Matrix(3, 1);
3505         calibrator.getBiasAsMatrix(b2);
3506         assertEquals(b1, b2);
3507         final Matrix ma1 = calibrator.getInitialMa();
3508         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3509         final Matrix ma2 = new Matrix(3, 3);
3510         calibrator.getInitialMa(ma2);
3511         assertEquals(ma1, ma2);
3512         assertSame(calibrator.getMeasurements(), measurements);
3513         assertTrue(calibrator.isCommonAxisUsed());
3514         assertNull(calibrator.getListener());
3515         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
3516         assertFalse(calibrator.isReady());
3517         assertFalse(calibrator.isRunning());
3518         assertNull(calibrator.getEstimatedMa());
3519         assertNull(calibrator.getEstimatedSx());
3520         assertNull(calibrator.getEstimatedSy());
3521         assertNull(calibrator.getEstimatedSz());
3522         assertNull(calibrator.getEstimatedMxy());
3523         assertNull(calibrator.getEstimatedMxz());
3524         assertNull(calibrator.getEstimatedMyx());
3525         assertNull(calibrator.getEstimatedMyz());
3526         assertNull(calibrator.getEstimatedMzx());
3527         assertNull(calibrator.getEstimatedMzy());
3528         assertNull(calibrator.getEstimatedCovariance());
3529         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3530         assertNull(calibrator.getGroundTruthGravityNorm());
3531         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3532         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3533     }
3534 
3535     @Test
3536     public void testConstructor39() throws WrongSizeException {
3537         final Collection<StandardDeviationBodyKinematics> measurements =
3538                 Collections.emptyList();
3539 
3540         final Matrix ba = generateBa();
3541         final double biasX = ba.getElementAtIndex(0);
3542         final double biasY = ba.getElementAtIndex(1);
3543         final double biasZ = ba.getElementAtIndex(2);
3544 
3545         final Matrix ma = generateMaCommonAxis();
3546         final double sx = ma.getElementAt(0, 0);
3547         final double sy = ma.getElementAt(1, 1);
3548         final double sz = ma.getElementAt(2, 2);
3549 
3550         final Acceleration bx = new Acceleration(biasX,
3551                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3552         final Acceleration by = new Acceleration(biasY,
3553                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3554         final Acceleration bz = new Acceleration(biasZ,
3555                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3556 
3557         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3558                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
3559                         true, bx, by, bz, sx, sy, sz,
3560                         this);
3561 
3562         // check default values
3563         assertEquals(calibrator.getBiasX(), biasX, 0.0);
3564         assertEquals(calibrator.getBiasY(), biasY, 0.0);
3565         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3566         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3567         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3568         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3569         final Acceleration bx2 = new Acceleration(0.0,
3570                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3571         calibrator.getBiasXAsAcceleration(bx2);
3572         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3573         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3574         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3575         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3576         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3577         final Acceleration by2 = new Acceleration(0.0,
3578                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3579         calibrator.getBiasYAsAcceleration(by2);
3580         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3581         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3582         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3583         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3584         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3585         final Acceleration bz2 = new Acceleration(0.0,
3586                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3587         calibrator.getBiasZAsAcceleration(bz2);
3588         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3589         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3590         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3591         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3592         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3593         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3594         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3595         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3596         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3597         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3598         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3599         final double[] bias1 = calibrator.getBias();
3600         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3601         final double[] bias2 = new double[3];
3602         calibrator.getBias(bias2);
3603         assertArrayEquals(bias1, bias2, 0.0);
3604         final Matrix b1 = calibrator.getBiasAsMatrix();
3605         assertEquals(b1, ba);
3606         final Matrix b2 = new Matrix(3, 1);
3607         calibrator.getBiasAsMatrix(b2);
3608         assertEquals(b1, b2);
3609         final Matrix ma1 = calibrator.getInitialMa();
3610         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3611         final Matrix ma2 = new Matrix(3, 3);
3612         calibrator.getInitialMa(ma2);
3613         assertEquals(ma1, ma2);
3614         assertSame(calibrator.getMeasurements(), measurements);
3615         assertTrue(calibrator.isCommonAxisUsed());
3616         assertSame(calibrator.getListener(), this);
3617         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
3618         assertFalse(calibrator.isReady());
3619         assertFalse(calibrator.isRunning());
3620         assertNull(calibrator.getEstimatedMa());
3621         assertNull(calibrator.getEstimatedSx());
3622         assertNull(calibrator.getEstimatedSy());
3623         assertNull(calibrator.getEstimatedSz());
3624         assertNull(calibrator.getEstimatedMxy());
3625         assertNull(calibrator.getEstimatedMxz());
3626         assertNull(calibrator.getEstimatedMyx());
3627         assertNull(calibrator.getEstimatedMyz());
3628         assertNull(calibrator.getEstimatedMzx());
3629         assertNull(calibrator.getEstimatedMzy());
3630         assertNull(calibrator.getEstimatedCovariance());
3631         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3632         assertNull(calibrator.getGroundTruthGravityNorm());
3633         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3634         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3635     }
3636 
3637     @Test
3638     public void testConstructor40() throws WrongSizeException {
3639         final Matrix ba = generateBa();
3640         final double biasX = ba.getElementAtIndex(0);
3641         final double biasY = ba.getElementAtIndex(1);
3642         final double biasZ = ba.getElementAtIndex(2);
3643 
3644         final Matrix ma = generateMaCommonAxis();
3645         final double sx = ma.getElementAt(0, 0);
3646         final double sy = ma.getElementAt(1, 1);
3647         final double sz = ma.getElementAt(2, 2);
3648         final double mxy = ma.getElementAt(0, 1);
3649         final double mxz = ma.getElementAt(0, 2);
3650         final double myx = ma.getElementAt(1, 0);
3651         final double myz = ma.getElementAt(1, 2);
3652         final double mzx = ma.getElementAt(2, 0);
3653         final double mzy = ma.getElementAt(2, 1);
3654 
3655         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3656                 new KnownBiasAndGravityNormAccelerometerCalibrator(
3657                         biasX, biasY, biasZ, sx, sy, sz,
3658                         mxy, mxz, myx, myz, mzx, mzy);
3659 
3660         // check default values
3661         assertEquals(calibrator.getBiasX(), biasX, 0.0);
3662         assertEquals(calibrator.getBiasY(), biasY, 0.0);
3663         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3664         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3665         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3666         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3667         final Acceleration bx2 = new Acceleration(0.0,
3668                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3669         calibrator.getBiasXAsAcceleration(bx2);
3670         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3671         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3672         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3673         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3674         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3675         final Acceleration by2 = new Acceleration(0.0,
3676                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3677         calibrator.getBiasYAsAcceleration(by2);
3678         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3679         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3680         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3681         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3682         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3683         final Acceleration bz2 = new Acceleration(0.0,
3684                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3685         calibrator.getBiasZAsAcceleration(bz2);
3686         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3687         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3688         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3689         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3690         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3691         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
3692         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
3693         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
3694         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
3695         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
3696         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
3697         final double[] bias1 = calibrator.getBias();
3698         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3699         final double[] bias2 = new double[3];
3700         calibrator.getBias(bias2);
3701         assertArrayEquals(bias1, bias2, 0.0);
3702         final Matrix b1 = calibrator.getBiasAsMatrix();
3703         assertEquals(b1, ba);
3704         final Matrix b2 = new Matrix(3, 1);
3705         calibrator.getBiasAsMatrix(b2);
3706         assertEquals(b1, b2);
3707         final Matrix ma1 = new Matrix(3, 3);
3708         ma1.setSubmatrix(0, 0,
3709                 2, 2,
3710                 new double[]{sx, myx, mzx,
3711                         mxy, sy, mzy,
3712                         mxz, myz, sz});
3713         assertEquals(calibrator.getInitialMa(), ma1);
3714         final Matrix ma2 = new Matrix(3, 3);
3715         calibrator.getInitialMa(ma2);
3716         assertEquals(ma1, ma2);
3717         assertNull(calibrator.getMeasurements());
3718         assertFalse(calibrator.isCommonAxisUsed());
3719         assertNull(calibrator.getListener());
3720         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3721         assertFalse(calibrator.isReady());
3722         assertFalse(calibrator.isRunning());
3723         assertNull(calibrator.getEstimatedMa());
3724         assertNull(calibrator.getEstimatedSx());
3725         assertNull(calibrator.getEstimatedSy());
3726         assertNull(calibrator.getEstimatedSz());
3727         assertNull(calibrator.getEstimatedMxy());
3728         assertNull(calibrator.getEstimatedMxz());
3729         assertNull(calibrator.getEstimatedMyx());
3730         assertNull(calibrator.getEstimatedMyz());
3731         assertNull(calibrator.getEstimatedMzx());
3732         assertNull(calibrator.getEstimatedMzy());
3733         assertNull(calibrator.getEstimatedCovariance());
3734         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3735         assertNull(calibrator.getGroundTruthGravityNorm());
3736         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3737         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3738     }
3739 
3740     @Test
3741     public void testConstructor41() throws WrongSizeException {
3742         final Collection<StandardDeviationBodyKinematics> measurements =
3743                 Collections.emptyList();
3744 
3745         final Matrix ba = generateBa();
3746         final double biasX = ba.getElementAtIndex(0);
3747         final double biasY = ba.getElementAtIndex(1);
3748         final double biasZ = ba.getElementAtIndex(2);
3749 
3750         final Matrix ma = generateMaCommonAxis();
3751         final double sx = ma.getElementAt(0, 0);
3752         final double sy = ma.getElementAt(1, 1);
3753         final double sz = ma.getElementAt(2, 2);
3754         final double mxy = ma.getElementAt(0, 1);
3755         final double mxz = ma.getElementAt(0, 2);
3756         final double myx = ma.getElementAt(1, 0);
3757         final double myz = ma.getElementAt(1, 2);
3758         final double mzx = ma.getElementAt(2, 0);
3759         final double mzy = ma.getElementAt(2, 1);
3760 
3761         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3762                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
3763                         biasX, biasY, biasZ, sx, sy, sz,
3764                         mxy, mxz, myx, myz, mzx, mzy);
3765 
3766         // check default values
3767         assertEquals(calibrator.getBiasX(), biasX, 0.0);
3768         assertEquals(calibrator.getBiasY(), biasY, 0.0);
3769         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3770         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3771         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3772         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3773         final Acceleration bx2 = new Acceleration(0.0,
3774                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3775         calibrator.getBiasXAsAcceleration(bx2);
3776         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3777         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3778         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3779         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3780         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3781         final Acceleration by2 = new Acceleration(0.0,
3782                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3783         calibrator.getBiasYAsAcceleration(by2);
3784         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3785         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3786         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3787         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3788         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3789         final Acceleration bz2 = new Acceleration(0.0,
3790                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3791         calibrator.getBiasZAsAcceleration(bz2);
3792         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3793         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3794         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3795         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3796         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3797         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
3798         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
3799         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
3800         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
3801         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
3802         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
3803         final double[] bias1 = calibrator.getBias();
3804         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3805         final double[] bias2 = new double[3];
3806         calibrator.getBias(bias2);
3807         assertArrayEquals(bias1, bias2, 0.0);
3808         final Matrix b1 = calibrator.getBiasAsMatrix();
3809         assertEquals(b1, ba);
3810         final Matrix b2 = new Matrix(3, 1);
3811         calibrator.getBiasAsMatrix(b2);
3812         assertEquals(b1, b2);
3813         final Matrix ma1 = new Matrix(3, 3);
3814         ma1.setSubmatrix(0, 0,
3815                 2, 2,
3816                 new double[]{sx, myx, mzx,
3817                         mxy, sy, mzy,
3818                         mxz, myz, sz});
3819         assertEquals(calibrator.getInitialMa(), ma1);
3820         final Matrix ma2 = new Matrix(3, 3);
3821         calibrator.getInitialMa(ma2);
3822         assertEquals(ma1, ma2);
3823         assertSame(calibrator.getMeasurements(), measurements);
3824         assertFalse(calibrator.isCommonAxisUsed());
3825         assertNull(calibrator.getListener());
3826         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3827         assertFalse(calibrator.isReady());
3828         assertFalse(calibrator.isRunning());
3829         assertNull(calibrator.getEstimatedMa());
3830         assertNull(calibrator.getEstimatedSx());
3831         assertNull(calibrator.getEstimatedSy());
3832         assertNull(calibrator.getEstimatedSz());
3833         assertNull(calibrator.getEstimatedMxy());
3834         assertNull(calibrator.getEstimatedMxz());
3835         assertNull(calibrator.getEstimatedMyx());
3836         assertNull(calibrator.getEstimatedMyz());
3837         assertNull(calibrator.getEstimatedMzx());
3838         assertNull(calibrator.getEstimatedMzy());
3839         assertNull(calibrator.getEstimatedCovariance());
3840         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3841         assertNull(calibrator.getGroundTruthGravityNorm());
3842         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3843         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3844     }
3845 
3846     @Test
3847     public void testConstructor42() throws WrongSizeException {
3848         final Collection<StandardDeviationBodyKinematics> measurements =
3849                 Collections.emptyList();
3850 
3851         final Matrix ba = generateBa();
3852         final double biasX = ba.getElementAtIndex(0);
3853         final double biasY = ba.getElementAtIndex(1);
3854         final double biasZ = ba.getElementAtIndex(2);
3855 
3856         final Matrix ma = generateMaCommonAxis();
3857         final double sx = ma.getElementAt(0, 0);
3858         final double sy = ma.getElementAt(1, 1);
3859         final double sz = ma.getElementAt(2, 2);
3860         final double mxy = ma.getElementAt(0, 1);
3861         final double mxz = ma.getElementAt(0, 2);
3862         final double myx = ma.getElementAt(1, 0);
3863         final double myz = ma.getElementAt(1, 2);
3864         final double mzx = ma.getElementAt(2, 0);
3865         final double mzy = ma.getElementAt(2, 1);
3866 
3867         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3868                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
3869                         biasX, biasY, biasZ, sx, sy, sz,
3870                         mxy, mxz, myx, myz, mzx, mzy, this);
3871 
3872         // check default values
3873         assertEquals(calibrator.getBiasX(), biasX, 0.0);
3874         assertEquals(calibrator.getBiasY(), biasY, 0.0);
3875         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3876         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3877         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3878         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3879         final Acceleration bx2 = new Acceleration(0.0,
3880                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3881         calibrator.getBiasXAsAcceleration(bx2);
3882         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3883         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3884         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3885         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3886         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3887         final Acceleration by2 = new Acceleration(0.0,
3888                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3889         calibrator.getBiasYAsAcceleration(by2);
3890         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3891         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3892         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3893         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3894         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3895         final Acceleration bz2 = new Acceleration(0.0,
3896                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3897         calibrator.getBiasZAsAcceleration(bz2);
3898         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3899         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3900         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3901         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3902         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3903         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
3904         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
3905         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
3906         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
3907         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
3908         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
3909         final double[] bias1 = calibrator.getBias();
3910         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3911         final double[] bias2 = new double[3];
3912         calibrator.getBias(bias2);
3913         assertArrayEquals(bias1, bias2, 0.0);
3914         final Matrix b1 = calibrator.getBiasAsMatrix();
3915         assertEquals(b1, ba);
3916         final Matrix b2 = new Matrix(3, 1);
3917         calibrator.getBiasAsMatrix(b2);
3918         assertEquals(b1, b2);
3919         final Matrix ma1 = new Matrix(3, 3);
3920         ma1.setSubmatrix(0, 0,
3921                 2, 2,
3922                 new double[]{sx, myx, mzx,
3923                         mxy, sy, mzy,
3924                         mxz, myz, sz});
3925         assertEquals(calibrator.getInitialMa(), ma1);
3926         final Matrix ma2 = new Matrix(3, 3);
3927         calibrator.getInitialMa(ma2);
3928         assertEquals(ma1, ma2);
3929         assertSame(calibrator.getMeasurements(), measurements);
3930         assertFalse(calibrator.isCommonAxisUsed());
3931         assertSame(calibrator.getListener(), this);
3932         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3933         assertFalse(calibrator.isReady());
3934         assertFalse(calibrator.isRunning());
3935         assertNull(calibrator.getEstimatedMa());
3936         assertNull(calibrator.getEstimatedSx());
3937         assertNull(calibrator.getEstimatedSy());
3938         assertNull(calibrator.getEstimatedSz());
3939         assertNull(calibrator.getEstimatedMxy());
3940         assertNull(calibrator.getEstimatedMxz());
3941         assertNull(calibrator.getEstimatedMyx());
3942         assertNull(calibrator.getEstimatedMyz());
3943         assertNull(calibrator.getEstimatedMzx());
3944         assertNull(calibrator.getEstimatedMzy());
3945         assertNull(calibrator.getEstimatedCovariance());
3946         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3947         assertNull(calibrator.getGroundTruthGravityNorm());
3948         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3949         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3950     }
3951 
3952     @Test
3953     public void testConstructor43() throws WrongSizeException {
3954         final Matrix ba = generateBa();
3955         final double biasX = ba.getElementAtIndex(0);
3956         final double biasY = ba.getElementAtIndex(1);
3957         final double biasZ = ba.getElementAtIndex(2);
3958 
3959         final Matrix ma = generateMaCommonAxis();
3960         final double sx = ma.getElementAt(0, 0);
3961         final double sy = ma.getElementAt(1, 1);
3962         final double sz = ma.getElementAt(2, 2);
3963         final double mxy = ma.getElementAt(0, 1);
3964         final double mxz = ma.getElementAt(0, 2);
3965         final double myx = ma.getElementAt(1, 0);
3966         final double myz = ma.getElementAt(1, 2);
3967         final double mzx = ma.getElementAt(2, 0);
3968         final double mzy = ma.getElementAt(2, 1);
3969 
3970         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
3971                 new KnownBiasAndGravityNormAccelerometerCalibrator(
3972                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
3973                         myz, mzx, mzy);
3974 
3975         // check default values
3976         assertEquals(calibrator.getBiasX(), biasX, 0.0);
3977         assertEquals(calibrator.getBiasY(), biasY, 0.0);
3978         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
3979         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
3980         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3981         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3982         final Acceleration bx2 = new Acceleration(0.0,
3983                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3984         calibrator.getBiasXAsAcceleration(bx2);
3985         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3986         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3987         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
3988         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3989         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3990         final Acceleration by2 = new Acceleration(0.0,
3991                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3992         calibrator.getBiasYAsAcceleration(by2);
3993         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3994         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3995         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
3996         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3997         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3998         final Acceleration bz2 = new Acceleration(0.0,
3999                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4000         calibrator.getBiasZAsAcceleration(bz2);
4001         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4002         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4003         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4004         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4005         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4006         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4007         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4008         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4009         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4010         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4011         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4012         final double[] bias1 = calibrator.getBias();
4013         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4014         final double[] bias2 = new double[3];
4015         calibrator.getBias(bias2);
4016         assertArrayEquals(bias1, bias2, 0.0);
4017         final Matrix b1 = calibrator.getBiasAsMatrix();
4018         assertEquals(b1, ba);
4019         final Matrix b2 = new Matrix(3, 1);
4020         calibrator.getBiasAsMatrix(b2);
4021         assertEquals(b1, b2);
4022         final Matrix ma1 = new Matrix(3, 3);
4023         ma1.setSubmatrix(0, 0,
4024                 2, 2,
4025                 new double[]{sx, myx, mzx,
4026                         mxy, sy, mzy,
4027                         mxz, myz, sz});
4028         assertEquals(calibrator.getInitialMa(), ma1);
4029         final Matrix ma2 = new Matrix(3, 3);
4030         calibrator.getInitialMa(ma2);
4031         assertEquals(ma1, ma2);
4032         assertNull(calibrator.getMeasurements());
4033         assertTrue(calibrator.isCommonAxisUsed());
4034         assertNull(calibrator.getListener());
4035         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
4036         assertFalse(calibrator.isReady());
4037         assertFalse(calibrator.isRunning());
4038         assertNull(calibrator.getEstimatedMa());
4039         assertNull(calibrator.getEstimatedSx());
4040         assertNull(calibrator.getEstimatedSy());
4041         assertNull(calibrator.getEstimatedSz());
4042         assertNull(calibrator.getEstimatedMxy());
4043         assertNull(calibrator.getEstimatedMxz());
4044         assertNull(calibrator.getEstimatedMyx());
4045         assertNull(calibrator.getEstimatedMyz());
4046         assertNull(calibrator.getEstimatedMzx());
4047         assertNull(calibrator.getEstimatedMzy());
4048         assertNull(calibrator.getEstimatedCovariance());
4049         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4050         assertNull(calibrator.getGroundTruthGravityNorm());
4051         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4052         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4053     }
4054 
4055     @Test
4056     public void testConstructor44() throws WrongSizeException {
4057         final Matrix ba = generateBa();
4058         final double biasX = ba.getElementAtIndex(0);
4059         final double biasY = ba.getElementAtIndex(1);
4060         final double biasZ = ba.getElementAtIndex(2);
4061 
4062         final Matrix ma = generateMaCommonAxis();
4063         final double sx = ma.getElementAt(0, 0);
4064         final double sy = ma.getElementAt(1, 1);
4065         final double sz = ma.getElementAt(2, 2);
4066         final double mxy = ma.getElementAt(0, 1);
4067         final double mxz = ma.getElementAt(0, 2);
4068         final double myx = ma.getElementAt(1, 0);
4069         final double myz = ma.getElementAt(1, 2);
4070         final double mzx = ma.getElementAt(2, 0);
4071         final double mzy = ma.getElementAt(2, 1);
4072 
4073         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
4074                 new KnownBiasAndGravityNormAccelerometerCalibrator(
4075                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
4076                         myz, mzx, mzy, this);
4077 
4078         // check default values
4079         assertEquals(calibrator.getBiasX(), biasX, 0.0);
4080         assertEquals(calibrator.getBiasY(), biasY, 0.0);
4081         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
4082         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
4083         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4084         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4085         final Acceleration bx2 = new Acceleration(0.0,
4086                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4087         calibrator.getBiasXAsAcceleration(bx2);
4088         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4089         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4090         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
4091         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4092         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4093         final Acceleration by2 = new Acceleration(0.0,
4094                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4095         calibrator.getBiasYAsAcceleration(by2);
4096         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4097         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4098         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
4099         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4100         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4101         final Acceleration bz2 = new Acceleration(0.0,
4102                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4103         calibrator.getBiasZAsAcceleration(bz2);
4104         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4105         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4106         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4107         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4108         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4109         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4110         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4111         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4112         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4113         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4114         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4115         final double[] bias1 = calibrator.getBias();
4116         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4117         final double[] bias2 = new double[3];
4118         calibrator.getBias(bias2);
4119         assertArrayEquals(bias1, bias2, 0.0);
4120         final Matrix b1 = calibrator.getBiasAsMatrix();
4121         assertEquals(b1, ba);
4122         final Matrix b2 = new Matrix(3, 1);
4123         calibrator.getBiasAsMatrix(b2);
4124         assertEquals(b1, b2);
4125         final Matrix ma1 = new Matrix(3, 3);
4126         ma1.setSubmatrix(0, 0,
4127                 2, 2,
4128                 new double[]{sx, myx, mzx,
4129                         mxy, sy, mzy,
4130                         mxz, myz, sz});
4131         assertEquals(calibrator.getInitialMa(), ma1);
4132         final Matrix ma2 = new Matrix(3, 3);
4133         calibrator.getInitialMa(ma2);
4134         assertEquals(ma1, ma2);
4135         assertNull(calibrator.getMeasurements());
4136         assertTrue(calibrator.isCommonAxisUsed());
4137         assertSame(calibrator.getListener(), this);
4138         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
4139         assertFalse(calibrator.isReady());
4140         assertFalse(calibrator.isRunning());
4141         assertNull(calibrator.getEstimatedMa());
4142         assertNull(calibrator.getEstimatedSx());
4143         assertNull(calibrator.getEstimatedSy());
4144         assertNull(calibrator.getEstimatedSz());
4145         assertNull(calibrator.getEstimatedMxy());
4146         assertNull(calibrator.getEstimatedMxz());
4147         assertNull(calibrator.getEstimatedMyx());
4148         assertNull(calibrator.getEstimatedMyz());
4149         assertNull(calibrator.getEstimatedMzx());
4150         assertNull(calibrator.getEstimatedMzy());
4151         assertNull(calibrator.getEstimatedCovariance());
4152         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4153         assertNull(calibrator.getGroundTruthGravityNorm());
4154         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4155         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4156     }
4157 
4158     @Test
4159     public void testConstructor45() throws WrongSizeException {
4160         final Collection<StandardDeviationBodyKinematics> measurements =
4161                 Collections.emptyList();
4162 
4163         final Matrix ba = generateBa();
4164         final double biasX = ba.getElementAtIndex(0);
4165         final double biasY = ba.getElementAtIndex(1);
4166         final double biasZ = ba.getElementAtIndex(2);
4167 
4168         final Matrix ma = generateMaCommonAxis();
4169         final double sx = ma.getElementAt(0, 0);
4170         final double sy = ma.getElementAt(1, 1);
4171         final double sz = ma.getElementAt(2, 2);
4172         final double mxy = ma.getElementAt(0, 1);
4173         final double mxz = ma.getElementAt(0, 2);
4174         final double myx = ma.getElementAt(1, 0);
4175         final double myz = ma.getElementAt(1, 2);
4176         final double mzx = ma.getElementAt(2, 0);
4177         final double mzy = ma.getElementAt(2, 1);
4178 
4179         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
4180                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
4181                         true, biasX, biasY, biasZ,
4182                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4183 
4184         // check default values
4185         assertEquals(calibrator.getBiasX(), biasX, 0.0);
4186         assertEquals(calibrator.getBiasY(), biasY, 0.0);
4187         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
4188         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
4189         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4190         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4191         final Acceleration bx2 = new Acceleration(0.0,
4192                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4193         calibrator.getBiasXAsAcceleration(bx2);
4194         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4195         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4196         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
4197         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4198         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4199         final Acceleration by2 = new Acceleration(0.0,
4200                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4201         calibrator.getBiasYAsAcceleration(by2);
4202         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4203         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4204         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
4205         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4206         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4207         final Acceleration bz2 = new Acceleration(0.0,
4208                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4209         calibrator.getBiasZAsAcceleration(bz2);
4210         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4211         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4212         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4213         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4214         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4215         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4216         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4217         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4218         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4219         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4220         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4221         final double[] bias1 = calibrator.getBias();
4222         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4223         final double[] bias2 = new double[3];
4224         calibrator.getBias(bias2);
4225         assertArrayEquals(bias1, bias2, 0.0);
4226         final Matrix b1 = calibrator.getBiasAsMatrix();
4227         assertEquals(b1, ba);
4228         final Matrix b2 = new Matrix(3, 1);
4229         calibrator.getBiasAsMatrix(b2);
4230         assertEquals(b1, b2);
4231         final Matrix ma1 = new Matrix(3, 3);
4232         ma1.setSubmatrix(0, 0,
4233                 2, 2,
4234                 new double[]{sx, myx, mzx,
4235                         mxy, sy, mzy,
4236                         mxz, myz, sz});
4237         assertEquals(calibrator.getInitialMa(), ma1);
4238         final Matrix ma2 = new Matrix(3, 3);
4239         calibrator.getInitialMa(ma2);
4240         assertEquals(ma1, ma2);
4241         assertSame(calibrator.getMeasurements(), measurements);
4242         assertTrue(calibrator.isCommonAxisUsed());
4243         assertNull(calibrator.getListener());
4244         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
4245         assertFalse(calibrator.isReady());
4246         assertFalse(calibrator.isRunning());
4247         assertNull(calibrator.getEstimatedMa());
4248         assertNull(calibrator.getEstimatedSx());
4249         assertNull(calibrator.getEstimatedSy());
4250         assertNull(calibrator.getEstimatedSz());
4251         assertNull(calibrator.getEstimatedMxy());
4252         assertNull(calibrator.getEstimatedMxz());
4253         assertNull(calibrator.getEstimatedMyx());
4254         assertNull(calibrator.getEstimatedMyz());
4255         assertNull(calibrator.getEstimatedMzx());
4256         assertNull(calibrator.getEstimatedMzy());
4257         assertNull(calibrator.getEstimatedCovariance());
4258         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4259         assertNull(calibrator.getGroundTruthGravityNorm());
4260         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4261         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4262     }
4263 
4264     @Test
4265     public void testConstructor46() throws WrongSizeException {
4266         final Collection<StandardDeviationBodyKinematics> measurements =
4267                 Collections.emptyList();
4268 
4269         final Matrix ba = generateBa();
4270         final double biasX = ba.getElementAtIndex(0);
4271         final double biasY = ba.getElementAtIndex(1);
4272         final double biasZ = ba.getElementAtIndex(2);
4273 
4274         final Matrix ma = generateMaCommonAxis();
4275         final double sx = ma.getElementAt(0, 0);
4276         final double sy = ma.getElementAt(1, 1);
4277         final double sz = ma.getElementAt(2, 2);
4278         final double mxy = ma.getElementAt(0, 1);
4279         final double mxz = ma.getElementAt(0, 2);
4280         final double myx = ma.getElementAt(1, 0);
4281         final double myz = ma.getElementAt(1, 2);
4282         final double mzx = ma.getElementAt(2, 0);
4283         final double mzy = ma.getElementAt(2, 1);
4284 
4285         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
4286                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
4287                         true, biasX, biasY, biasZ,
4288                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
4289 
4290         // check default values
4291         assertEquals(calibrator.getBiasX(), biasX, 0.0);
4292         assertEquals(calibrator.getBiasY(), biasY, 0.0);
4293         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
4294         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
4295         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4296         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4297         final Acceleration bx2 = new Acceleration(0.0,
4298                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4299         calibrator.getBiasXAsAcceleration(bx2);
4300         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4301         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4302         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
4303         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4304         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4305         final Acceleration by2 = new Acceleration(0.0,
4306                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4307         calibrator.getBiasYAsAcceleration(by2);
4308         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4309         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4310         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
4311         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4312         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4313         final Acceleration bz2 = new Acceleration(0.0,
4314                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4315         calibrator.getBiasZAsAcceleration(bz2);
4316         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4317         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4318         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4319         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4320         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4321         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4322         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4323         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4324         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4325         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4326         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4327         final double[] bias1 = calibrator.getBias();
4328         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4329         final double[] bias2 = new double[3];
4330         calibrator.getBias(bias2);
4331         assertArrayEquals(bias1, bias2, 0.0);
4332         final Matrix b1 = calibrator.getBiasAsMatrix();
4333         assertEquals(b1, ba);
4334         final Matrix b2 = new Matrix(3, 1);
4335         calibrator.getBiasAsMatrix(b2);
4336         assertEquals(b1, b2);
4337         final Matrix ma1 = new Matrix(3, 3);
4338         ma1.setSubmatrix(0, 0,
4339                 2, 2,
4340                 new double[]{sx, myx, mzx,
4341                         mxy, sy, mzy,
4342                         mxz, myz, sz});
4343         assertEquals(calibrator.getInitialMa(), ma1);
4344         final Matrix ma2 = new Matrix(3, 3);
4345         calibrator.getInitialMa(ma2);
4346         assertEquals(ma1, ma2);
4347         assertSame(calibrator.getMeasurements(), measurements);
4348         assertTrue(calibrator.isCommonAxisUsed());
4349         assertSame(calibrator.getListener(), this);
4350         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
4351         assertFalse(calibrator.isReady());
4352         assertFalse(calibrator.isRunning());
4353         assertNull(calibrator.getEstimatedMa());
4354         assertNull(calibrator.getEstimatedSx());
4355         assertNull(calibrator.getEstimatedSy());
4356         assertNull(calibrator.getEstimatedSz());
4357         assertNull(calibrator.getEstimatedMxy());
4358         assertNull(calibrator.getEstimatedMxz());
4359         assertNull(calibrator.getEstimatedMyx());
4360         assertNull(calibrator.getEstimatedMyz());
4361         assertNull(calibrator.getEstimatedMzx());
4362         assertNull(calibrator.getEstimatedMzy());
4363         assertNull(calibrator.getEstimatedCovariance());
4364         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4365         assertNull(calibrator.getGroundTruthGravityNorm());
4366         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4367         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4368     }
4369 
4370     @Test
4371     public void testConstructor47() throws WrongSizeException {
4372         final Matrix ba = generateBa();
4373         final double biasX = ba.getElementAtIndex(0);
4374         final double biasY = ba.getElementAtIndex(1);
4375         final double biasZ = ba.getElementAtIndex(2);
4376 
4377         final Matrix ma = generateMaCommonAxis();
4378         final double sx = ma.getElementAt(0, 0);
4379         final double sy = ma.getElementAt(1, 1);
4380         final double sz = ma.getElementAt(2, 2);
4381         final double mxy = ma.getElementAt(0, 1);
4382         final double mxz = ma.getElementAt(0, 2);
4383         final double myx = ma.getElementAt(1, 0);
4384         final double myz = ma.getElementAt(1, 2);
4385         final double mzx = ma.getElementAt(2, 0);
4386         final double mzy = ma.getElementAt(2, 1);
4387 
4388         final Acceleration bx = new Acceleration(biasX,
4389                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4390         final Acceleration by = new Acceleration(biasY,
4391                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4392         final Acceleration bz = new Acceleration(biasZ,
4393                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4394 
4395         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
4396                 new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz,
4397                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4398 
4399         // check default values
4400         assertEquals(calibrator.getBiasX(), biasX, 0.0);
4401         assertEquals(calibrator.getBiasY(), biasY, 0.0);
4402         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
4403         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
4404         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4405         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4406         final Acceleration bx2 = new Acceleration(0.0,
4407                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4408         calibrator.getBiasXAsAcceleration(bx2);
4409         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4410         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4411         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
4412         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4413         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4414         final Acceleration by2 = new Acceleration(0.0,
4415                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4416         calibrator.getBiasYAsAcceleration(by2);
4417         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4418         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4419         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
4420         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4421         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4422         final Acceleration bz2 = new Acceleration(0.0,
4423                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4424         calibrator.getBiasZAsAcceleration(bz2);
4425         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4426         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4427         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4428         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4429         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4430         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4431         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4432         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4433         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4434         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4435         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4436         final double[] bias1 = calibrator.getBias();
4437         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4438         final double[] bias2 = new double[3];
4439         calibrator.getBias(bias2);
4440         assertArrayEquals(bias1, bias2, 0.0);
4441         final Matrix b1 = calibrator.getBiasAsMatrix();
4442         assertEquals(b1, ba);
4443         final Matrix b2 = new Matrix(3, 1);
4444         calibrator.getBiasAsMatrix(b2);
4445         assertEquals(b1, b2);
4446         final Matrix ma1 = new Matrix(3, 3);
4447         ma1.setSubmatrix(0, 0,
4448                 2, 2,
4449                 new double[]{sx, myx, mzx,
4450                         mxy, sy, mzy,
4451                         mxz, myz, sz});
4452         assertEquals(calibrator.getInitialMa(), ma1);
4453         final Matrix ma2 = new Matrix(3, 3);
4454         calibrator.getInitialMa(ma2);
4455         assertEquals(ma1, ma2);
4456         assertNull(calibrator.getMeasurements());
4457         assertFalse(calibrator.isCommonAxisUsed());
4458         assertNull(calibrator.getListener());
4459         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4460         assertFalse(calibrator.isReady());
4461         assertFalse(calibrator.isRunning());
4462         assertNull(calibrator.getEstimatedMa());
4463         assertNull(calibrator.getEstimatedSx());
4464         assertNull(calibrator.getEstimatedSy());
4465         assertNull(calibrator.getEstimatedSz());
4466         assertNull(calibrator.getEstimatedMxy());
4467         assertNull(calibrator.getEstimatedMxz());
4468         assertNull(calibrator.getEstimatedMyx());
4469         assertNull(calibrator.getEstimatedMyz());
4470         assertNull(calibrator.getEstimatedMzx());
4471         assertNull(calibrator.getEstimatedMzy());
4472         assertNull(calibrator.getEstimatedCovariance());
4473         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4474         assertNull(calibrator.getGroundTruthGravityNorm());
4475         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4476         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4477     }
4478 
4479     @Test
4480     public void testConstructor48() throws WrongSizeException {
4481         final Matrix ba = generateBa();
4482         final double biasX = ba.getElementAtIndex(0);
4483         final double biasY = ba.getElementAtIndex(1);
4484         final double biasZ = ba.getElementAtIndex(2);
4485 
4486         final Matrix ma = generateMaCommonAxis();
4487         final double sx = ma.getElementAt(0, 0);
4488         final double sy = ma.getElementAt(1, 1);
4489         final double sz = ma.getElementAt(2, 2);
4490         final double mxy = ma.getElementAt(0, 1);
4491         final double mxz = ma.getElementAt(0, 2);
4492         final double myx = ma.getElementAt(1, 0);
4493         final double myz = ma.getElementAt(1, 2);
4494         final double mzx = ma.getElementAt(2, 0);
4495         final double mzy = ma.getElementAt(2, 1);
4496 
4497         final Acceleration bx = new Acceleration(biasX,
4498                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4499         final Acceleration by = new Acceleration(biasY,
4500                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4501         final Acceleration bz = new Acceleration(biasZ,
4502                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4503 
4504         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
4505                 new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz,
4506                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
4507 
4508         // check default values
4509         assertEquals(calibrator.getBiasX(), biasX, 0.0);
4510         assertEquals(calibrator.getBiasY(), biasY, 0.0);
4511         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
4512         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
4513         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4514         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4515         final Acceleration bx2 = new Acceleration(0.0,
4516                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4517         calibrator.getBiasXAsAcceleration(bx2);
4518         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4519         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4520         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
4521         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4522         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4523         final Acceleration by2 = new Acceleration(0.0,
4524                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4525         calibrator.getBiasYAsAcceleration(by2);
4526         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4527         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4528         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
4529         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4530         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4531         final Acceleration bz2 = new Acceleration(0.0,
4532                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4533         calibrator.getBiasZAsAcceleration(bz2);
4534         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4535         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4536         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4537         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4538         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4539         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4540         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4541         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4542         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4543         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4544         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4545         final double[] bias1 = calibrator.getBias();
4546         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4547         final double[] bias2 = new double[3];
4548         calibrator.getBias(bias2);
4549         assertArrayEquals(bias1, bias2, 0.0);
4550         final Matrix b1 = calibrator.getBiasAsMatrix();
4551         assertEquals(b1, ba);
4552         final Matrix b2 = new Matrix(3, 1);
4553         calibrator.getBiasAsMatrix(b2);
4554         assertEquals(b1, b2);
4555         final Matrix ma1 = new Matrix(3, 3);
4556         ma1.setSubmatrix(0, 0,
4557                 2, 2,
4558                 new double[]{sx, myx, mzx,
4559                         mxy, sy, mzy,
4560                         mxz, myz, sz});
4561         assertEquals(calibrator.getInitialMa(), ma1);
4562         final Matrix ma2 = new Matrix(3, 3);
4563         calibrator.getInitialMa(ma2);
4564         assertEquals(ma1, ma2);
4565         assertNull(calibrator.getMeasurements());
4566         assertFalse(calibrator.isCommonAxisUsed());
4567         assertSame(calibrator.getListener(), this);
4568         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4569         assertFalse(calibrator.isReady());
4570         assertFalse(calibrator.isRunning());
4571         assertNull(calibrator.getEstimatedMa());
4572         assertNull(calibrator.getEstimatedSx());
4573         assertNull(calibrator.getEstimatedSy());
4574         assertNull(calibrator.getEstimatedSz());
4575         assertNull(calibrator.getEstimatedMxy());
4576         assertNull(calibrator.getEstimatedMxz());
4577         assertNull(calibrator.getEstimatedMyx());
4578         assertNull(calibrator.getEstimatedMyz());
4579         assertNull(calibrator.getEstimatedMzx());
4580         assertNull(calibrator.getEstimatedMzy());
4581         assertNull(calibrator.getEstimatedCovariance());
4582         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4583         assertNull(calibrator.getGroundTruthGravityNorm());
4584         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4585         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4586     }
4587 
4588     @Test
4589     public void testConstructor49() throws WrongSizeException {
4590 
4591         final Collection<StandardDeviationBodyKinematics> measurements =
4592                 Collections.emptyList();
4593 
4594         final Matrix ba = generateBa();
4595         final double biasX = ba.getElementAtIndex(0);
4596         final double biasY = ba.getElementAtIndex(1);
4597         final double biasZ = ba.getElementAtIndex(2);
4598 
4599         final Matrix ma = generateMaCommonAxis();
4600         final double sx = ma.getElementAt(0, 0);
4601         final double sy = ma.getElementAt(1, 1);
4602         final double sz = ma.getElementAt(2, 2);
4603         final double mxy = ma.getElementAt(0, 1);
4604         final double mxz = ma.getElementAt(0, 2);
4605         final double myx = ma.getElementAt(1, 0);
4606         final double myz = ma.getElementAt(1, 2);
4607         final double mzx = ma.getElementAt(2, 0);
4608         final double mzy = ma.getElementAt(2, 1);
4609 
4610         final Acceleration bx = new Acceleration(biasX,
4611                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4612         final Acceleration by = new Acceleration(biasY,
4613                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4614         final Acceleration bz = new Acceleration(biasZ,
4615                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4616 
4617         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
4618                 new KnownBiasAndGravityNormAccelerometerCalibrator(
4619                         measurements, bx, by, bz, sx, sy, sz,
4620                         mxy, mxz, myx, myz, mzx, mzy);
4621 
4622         // check default values
4623         assertEquals(calibrator.getBiasX(), biasX, 0.0);
4624         assertEquals(calibrator.getBiasY(), biasY, 0.0);
4625         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
4626         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
4627         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4628         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4629         final Acceleration bx2 = new Acceleration(0.0,
4630                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4631         calibrator.getBiasXAsAcceleration(bx2);
4632         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4633         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4634         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
4635         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4636         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4637         final Acceleration by2 = new Acceleration(0.0,
4638                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4639         calibrator.getBiasYAsAcceleration(by2);
4640         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4641         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4642         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
4643         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4644         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4645         final Acceleration bz2 = new Acceleration(0.0,
4646                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4647         calibrator.getBiasZAsAcceleration(bz2);
4648         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4649         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4650         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4651         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4652         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4653         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4654         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4655         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4656         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4657         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4658         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4659         final double[] bias1 = calibrator.getBias();
4660         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4661         final double[] bias2 = new double[3];
4662         calibrator.getBias(bias2);
4663         assertArrayEquals(bias1, bias2, 0.0);
4664         final Matrix b1 = calibrator.getBiasAsMatrix();
4665         assertEquals(b1, ba);
4666         final Matrix b2 = new Matrix(3, 1);
4667         calibrator.getBiasAsMatrix(b2);
4668         assertEquals(b1, b2);
4669         final Matrix ma1 = new Matrix(3, 3);
4670         ma1.setSubmatrix(0, 0,
4671                 2, 2,
4672                 new double[]{sx, myx, mzx,
4673                         mxy, sy, mzy,
4674                         mxz, myz, sz});
4675         assertEquals(calibrator.getInitialMa(), ma1);
4676         final Matrix ma2 = new Matrix(3, 3);
4677         calibrator.getInitialMa(ma2);
4678         assertEquals(ma1, ma2);
4679         assertSame(calibrator.getMeasurements(), measurements);
4680         assertFalse(calibrator.isCommonAxisUsed());
4681         assertNull(calibrator.getListener());
4682         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4683         assertFalse(calibrator.isReady());
4684         assertFalse(calibrator.isRunning());
4685         assertNull(calibrator.getEstimatedMa());
4686         assertNull(calibrator.getEstimatedSx());
4687         assertNull(calibrator.getEstimatedSy());
4688         assertNull(calibrator.getEstimatedSz());
4689         assertNull(calibrator.getEstimatedMxy());
4690         assertNull(calibrator.getEstimatedMxz());
4691         assertNull(calibrator.getEstimatedMyx());
4692         assertNull(calibrator.getEstimatedMyz());
4693         assertNull(calibrator.getEstimatedMzx());
4694         assertNull(calibrator.getEstimatedMzy());
4695         assertNull(calibrator.getEstimatedCovariance());
4696         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4697         assertNull(calibrator.getGroundTruthGravityNorm());
4698         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4699         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4700     }
4701 
4702     @Test
4703     public void testConstructor50() throws WrongSizeException {
4704 
4705         final Collection<StandardDeviationBodyKinematics> measurements =
4706                 Collections.emptyList();
4707 
4708         final Matrix ba = generateBa();
4709         final double biasX = ba.getElementAtIndex(0);
4710         final double biasY = ba.getElementAtIndex(1);
4711         final double biasZ = ba.getElementAtIndex(2);
4712 
4713         final Matrix ma = generateMaCommonAxis();
4714         final double sx = ma.getElementAt(0, 0);
4715         final double sy = ma.getElementAt(1, 1);
4716         final double sz = ma.getElementAt(2, 2);
4717         final double mxy = ma.getElementAt(0, 1);
4718         final double mxz = ma.getElementAt(0, 2);
4719         final double myx = ma.getElementAt(1, 0);
4720         final double myz = ma.getElementAt(1, 2);
4721         final double mzx = ma.getElementAt(2, 0);
4722         final double mzy = ma.getElementAt(2, 1);
4723 
4724         final Acceleration bx = new Acceleration(biasX,
4725                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4726         final Acceleration by = new Acceleration(biasY,
4727                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4728         final Acceleration bz = new Acceleration(biasZ,
4729                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4730 
4731         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
4732                 new KnownBiasAndGravityNormAccelerometerCalibrator(
4733                         measurements, bx, by, bz, sx, sy, sz,
4734                         mxy, mxz, myx, myz, mzx, mzy, this);
4735 
4736         // check default values
4737         assertEquals(calibrator.getBiasX(), biasX, 0.0);
4738         assertEquals(calibrator.getBiasY(), biasY, 0.0);
4739         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
4740         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
4741         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4742         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4743         final Acceleration bx2 = new Acceleration(0.0,
4744                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4745         calibrator.getBiasXAsAcceleration(bx2);
4746         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4747         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4748         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
4749         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4750         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4751         final Acceleration by2 = new Acceleration(0.0,
4752                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4753         calibrator.getBiasYAsAcceleration(by2);
4754         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4755         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4756         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
4757         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4758         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4759         final Acceleration bz2 = new Acceleration(0.0,
4760                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4761         calibrator.getBiasZAsAcceleration(bz2);
4762         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4763         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4764         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4765         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4766         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4767         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4768         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4769         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4770         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4771         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4772         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4773         final double[] bias1 = calibrator.getBias();
4774         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4775         final double[] bias2 = new double[3];
4776         calibrator.getBias(bias2);
4777         assertArrayEquals(bias1, bias2, 0.0);
4778         final Matrix b1 = calibrator.getBiasAsMatrix();
4779         assertEquals(b1, ba);
4780         final Matrix b2 = new Matrix(3, 1);
4781         calibrator.getBiasAsMatrix(b2);
4782         assertEquals(b1, b2);
4783         final Matrix ma1 = new Matrix(3, 3);
4784         ma1.setSubmatrix(0, 0,
4785                 2, 2,
4786                 new double[]{sx, myx, mzx,
4787                         mxy, sy, mzy,
4788                         mxz, myz, sz});
4789         assertEquals(calibrator.getInitialMa(), ma1);
4790         final Matrix ma2 = new Matrix(3, 3);
4791         calibrator.getInitialMa(ma2);
4792         assertEquals(ma1, ma2);
4793         assertSame(calibrator.getMeasurements(), measurements);
4794         assertFalse(calibrator.isCommonAxisUsed());
4795         assertSame(calibrator.getListener(), this);
4796         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4797         assertFalse(calibrator.isReady());
4798         assertFalse(calibrator.isRunning());
4799         assertNull(calibrator.getEstimatedMa());
4800         assertNull(calibrator.getEstimatedSx());
4801         assertNull(calibrator.getEstimatedSy());
4802         assertNull(calibrator.getEstimatedSz());
4803         assertNull(calibrator.getEstimatedMxy());
4804         assertNull(calibrator.getEstimatedMxz());
4805         assertNull(calibrator.getEstimatedMyx());
4806         assertNull(calibrator.getEstimatedMyz());
4807         assertNull(calibrator.getEstimatedMzx());
4808         assertNull(calibrator.getEstimatedMzy());
4809         assertNull(calibrator.getEstimatedCovariance());
4810         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4811         assertNull(calibrator.getGroundTruthGravityNorm());
4812         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4813         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4814     }
4815 
4816     @Test
4817     public void testConstructor51() throws WrongSizeException {
4818         final Matrix ba = generateBa();
4819         final double biasX = ba.getElementAtIndex(0);
4820         final double biasY = ba.getElementAtIndex(1);
4821         final double biasZ = ba.getElementAtIndex(2);
4822 
4823         final Matrix ma = generateMaCommonAxis();
4824         final double sx = ma.getElementAt(0, 0);
4825         final double sy = ma.getElementAt(1, 1);
4826         final double sz = ma.getElementAt(2, 2);
4827         final double mxy = ma.getElementAt(0, 1);
4828         final double mxz = ma.getElementAt(0, 2);
4829         final double myx = ma.getElementAt(1, 0);
4830         final double myz = ma.getElementAt(1, 2);
4831         final double mzx = ma.getElementAt(2, 0);
4832         final double mzy = ma.getElementAt(2, 1);
4833 
4834         final Acceleration bx = new Acceleration(biasX,
4835                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4836         final Acceleration by = new Acceleration(biasY,
4837                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4838         final Acceleration bz = new Acceleration(biasZ,
4839                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4840 
4841         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
4842                 new KnownBiasAndGravityNormAccelerometerCalibrator(
4843                         true, bx, by, bz, sx, sy, sz,
4844                         mxy, mxz, myx, myz, mzx, mzy);
4845 
4846         // check default values
4847         assertEquals(calibrator.getBiasX(), biasX, 0.0);
4848         assertEquals(calibrator.getBiasY(), biasY, 0.0);
4849         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
4850         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
4851         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4852         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4853         final Acceleration bx2 = new Acceleration(0.0,
4854                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4855         calibrator.getBiasXAsAcceleration(bx2);
4856         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4857         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4858         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
4859         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4860         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4861         final Acceleration by2 = new Acceleration(0.0,
4862                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4863         calibrator.getBiasYAsAcceleration(by2);
4864         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4865         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4866         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
4867         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4868         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4869         final Acceleration bz2 = new Acceleration(0.0,
4870                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4871         calibrator.getBiasZAsAcceleration(bz2);
4872         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4873         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4874         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4875         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4876         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4877         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4878         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4879         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4880         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4881         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4882         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4883         final double[] bias1 = calibrator.getBias();
4884         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4885         final double[] bias2 = new double[3];
4886         calibrator.getBias(bias2);
4887         assertArrayEquals(bias1, bias2, 0.0);
4888         final Matrix b1 = calibrator.getBiasAsMatrix();
4889         assertEquals(b1, ba);
4890         final Matrix b2 = new Matrix(3, 1);
4891         calibrator.getBiasAsMatrix(b2);
4892         assertEquals(b1, b2);
4893         final Matrix ma1 = new Matrix(3, 3);
4894         ma1.setSubmatrix(0, 0,
4895                 2, 2,
4896                 new double[]{sx, myx, mzx,
4897                         mxy, sy, mzy,
4898                         mxz, myz, sz});
4899         assertEquals(calibrator.getInitialMa(), ma1);
4900         final Matrix ma2 = new Matrix(3, 3);
4901         calibrator.getInitialMa(ma2);
4902         assertEquals(ma1, ma2);
4903         assertNull(calibrator.getMeasurements());
4904         assertTrue(calibrator.isCommonAxisUsed());
4905         assertNull(calibrator.getListener());
4906         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
4907         assertFalse(calibrator.isReady());
4908         assertFalse(calibrator.isRunning());
4909         assertNull(calibrator.getEstimatedMa());
4910         assertNull(calibrator.getEstimatedSx());
4911         assertNull(calibrator.getEstimatedSy());
4912         assertNull(calibrator.getEstimatedSz());
4913         assertNull(calibrator.getEstimatedMxy());
4914         assertNull(calibrator.getEstimatedMxz());
4915         assertNull(calibrator.getEstimatedMyx());
4916         assertNull(calibrator.getEstimatedMyz());
4917         assertNull(calibrator.getEstimatedMzx());
4918         assertNull(calibrator.getEstimatedMzy());
4919         assertNull(calibrator.getEstimatedCovariance());
4920         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4921         assertNull(calibrator.getGroundTruthGravityNorm());
4922         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4923         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4924     }
4925 
4926     @Test
4927     public void testConstructor52() throws WrongSizeException {
4928         final Matrix ba = generateBa();
4929         final double biasX = ba.getElementAtIndex(0);
4930         final double biasY = ba.getElementAtIndex(1);
4931         final double biasZ = ba.getElementAtIndex(2);
4932 
4933         final Matrix ma = generateMaCommonAxis();
4934         final double sx = ma.getElementAt(0, 0);
4935         final double sy = ma.getElementAt(1, 1);
4936         final double sz = ma.getElementAt(2, 2);
4937         final double mxy = ma.getElementAt(0, 1);
4938         final double mxz = ma.getElementAt(0, 2);
4939         final double myx = ma.getElementAt(1, 0);
4940         final double myz = ma.getElementAt(1, 2);
4941         final double mzx = ma.getElementAt(2, 0);
4942         final double mzy = ma.getElementAt(2, 1);
4943 
4944         final Acceleration bx = new Acceleration(biasX,
4945                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4946         final Acceleration by = new Acceleration(biasY,
4947                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4948         final Acceleration bz = new Acceleration(biasZ,
4949                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4950 
4951         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
4952                 new KnownBiasAndGravityNormAccelerometerCalibrator(true,
4953                         bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
4954                         this);
4955 
4956         // check default values
4957         assertEquals(calibrator.getBiasX(), biasX, 0.0);
4958         assertEquals(calibrator.getBiasY(), biasY, 0.0);
4959         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
4960         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
4961         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4962         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4963         final Acceleration bx2 = new Acceleration(0.0,
4964                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4965         calibrator.getBiasXAsAcceleration(bx2);
4966         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4967         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4968         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
4969         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4970         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4971         final Acceleration by2 = new Acceleration(0.0,
4972                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4973         calibrator.getBiasYAsAcceleration(by2);
4974         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4975         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4976         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
4977         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4978         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4979         final Acceleration bz2 = new Acceleration(0.0,
4980                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4981         calibrator.getBiasZAsAcceleration(bz2);
4982         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4983         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4984         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4985         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4986         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4987         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4988         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4989         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4990         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4991         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4992         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4993         final double[] bias1 = calibrator.getBias();
4994         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4995         final double[] bias2 = new double[3];
4996         calibrator.getBias(bias2);
4997         assertArrayEquals(bias1, bias2, 0.0);
4998         final Matrix b1 = calibrator.getBiasAsMatrix();
4999         assertEquals(b1, ba);
5000         final Matrix b2 = new Matrix(3, 1);
5001         calibrator.getBiasAsMatrix(b2);
5002         assertEquals(b1, b2);
5003         final Matrix ma1 = new Matrix(3, 3);
5004         ma1.setSubmatrix(0, 0,
5005                 2, 2,
5006                 new double[]{sx, myx, mzx,
5007                         mxy, sy, mzy,
5008                         mxz, myz, sz});
5009         assertEquals(calibrator.getInitialMa(), ma1);
5010         final Matrix ma2 = new Matrix(3, 3);
5011         calibrator.getInitialMa(ma2);
5012         assertEquals(ma1, ma2);
5013         assertNull(calibrator.getMeasurements());
5014         assertTrue(calibrator.isCommonAxisUsed());
5015         assertSame(calibrator.getListener(), this);
5016         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
5017         assertFalse(calibrator.isReady());
5018         assertFalse(calibrator.isRunning());
5019         assertNull(calibrator.getEstimatedMa());
5020         assertNull(calibrator.getEstimatedSx());
5021         assertNull(calibrator.getEstimatedSy());
5022         assertNull(calibrator.getEstimatedSz());
5023         assertNull(calibrator.getEstimatedMxy());
5024         assertNull(calibrator.getEstimatedMxz());
5025         assertNull(calibrator.getEstimatedMyx());
5026         assertNull(calibrator.getEstimatedMyz());
5027         assertNull(calibrator.getEstimatedMzx());
5028         assertNull(calibrator.getEstimatedMzy());
5029         assertNull(calibrator.getEstimatedCovariance());
5030         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5031         assertNull(calibrator.getGroundTruthGravityNorm());
5032         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5033         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5034     }
5035 
5036     @Test
5037     public void testConstructor53() throws WrongSizeException {
5038         final Collection<StandardDeviationBodyKinematics> measurements =
5039                 Collections.emptyList();
5040 
5041         final Matrix ba = generateBa();
5042         final double biasX = ba.getElementAtIndex(0);
5043         final double biasY = ba.getElementAtIndex(1);
5044         final double biasZ = ba.getElementAtIndex(2);
5045 
5046         final Matrix ma = generateMaCommonAxis();
5047         final double sx = ma.getElementAt(0, 0);
5048         final double sy = ma.getElementAt(1, 1);
5049         final double sz = ma.getElementAt(2, 2);
5050         final double mxy = ma.getElementAt(0, 1);
5051         final double mxz = ma.getElementAt(0, 2);
5052         final double myx = ma.getElementAt(1, 0);
5053         final double myz = ma.getElementAt(1, 2);
5054         final double mzx = ma.getElementAt(2, 0);
5055         final double mzy = ma.getElementAt(2, 1);
5056 
5057         final Acceleration bx = new Acceleration(biasX,
5058                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5059         final Acceleration by = new Acceleration(biasY,
5060                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5061         final Acceleration bz = new Acceleration(biasZ,
5062                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5063 
5064         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5065                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
5066                         true, bx, by, bz, sx, sy, sz,
5067                         mxy, mxz, myx, myz, mzx, mzy);
5068 
5069         // check default values
5070         assertEquals(calibrator.getBiasX(), biasX, 0.0);
5071         assertEquals(calibrator.getBiasY(), biasY, 0.0);
5072         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5073         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5074         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5075         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5076         final Acceleration bx2 = new Acceleration(0.0,
5077                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5078         calibrator.getBiasXAsAcceleration(bx2);
5079         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5080         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5081         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5082         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5083         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5084         final Acceleration by2 = new Acceleration(0.0,
5085                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5086         calibrator.getBiasYAsAcceleration(by2);
5087         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5088         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5089         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5090         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5091         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5092         final Acceleration bz2 = new Acceleration(0.0,
5093                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5094         calibrator.getBiasZAsAcceleration(bz2);
5095         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5096         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5097         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5098         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5099         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5100         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5101         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5102         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5103         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5104         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5105         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5106         final double[] bias1 = calibrator.getBias();
5107         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5108         final double[] bias2 = new double[3];
5109         calibrator.getBias(bias2);
5110         assertArrayEquals(bias1, bias2, 0.0);
5111         final Matrix b1 = calibrator.getBiasAsMatrix();
5112         assertEquals(b1, ba);
5113         final Matrix b2 = new Matrix(3, 1);
5114         calibrator.getBiasAsMatrix(b2);
5115         assertEquals(b1, b2);
5116         final Matrix ma1 = new Matrix(3, 3);
5117         ma1.setSubmatrix(0, 0,
5118                 2, 2,
5119                 new double[]{sx, myx, mzx,
5120                         mxy, sy, mzy,
5121                         mxz, myz, sz});
5122         assertEquals(calibrator.getInitialMa(), ma1);
5123         final Matrix ma2 = new Matrix(3, 3);
5124         calibrator.getInitialMa(ma2);
5125         assertEquals(ma1, ma2);
5126         assertSame(calibrator.getMeasurements(), measurements);
5127         assertTrue(calibrator.isCommonAxisUsed());
5128         assertNull(calibrator.getListener());
5129         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
5130         assertFalse(calibrator.isReady());
5131         assertFalse(calibrator.isRunning());
5132         assertNull(calibrator.getEstimatedMa());
5133         assertNull(calibrator.getEstimatedSx());
5134         assertNull(calibrator.getEstimatedSy());
5135         assertNull(calibrator.getEstimatedSz());
5136         assertNull(calibrator.getEstimatedMxy());
5137         assertNull(calibrator.getEstimatedMxz());
5138         assertNull(calibrator.getEstimatedMyx());
5139         assertNull(calibrator.getEstimatedMyz());
5140         assertNull(calibrator.getEstimatedMzx());
5141         assertNull(calibrator.getEstimatedMzy());
5142         assertNull(calibrator.getEstimatedCovariance());
5143         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5144         assertNull(calibrator.getGroundTruthGravityNorm());
5145         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5146         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5147     }
5148 
5149     @Test
5150     public void testConstructor54() throws WrongSizeException {
5151         final Collection<StandardDeviationBodyKinematics> measurements =
5152                 Collections.emptyList();
5153 
5154         final Matrix ba = generateBa();
5155         final double biasX = ba.getElementAtIndex(0);
5156         final double biasY = ba.getElementAtIndex(1);
5157         final double biasZ = ba.getElementAtIndex(2);
5158 
5159         final Matrix ma = generateMaCommonAxis();
5160         final double sx = ma.getElementAt(0, 0);
5161         final double sy = ma.getElementAt(1, 1);
5162         final double sz = ma.getElementAt(2, 2);
5163         final double mxy = ma.getElementAt(0, 1);
5164         final double mxz = ma.getElementAt(0, 2);
5165         final double myx = ma.getElementAt(1, 0);
5166         final double myz = ma.getElementAt(1, 2);
5167         final double mzx = ma.getElementAt(2, 0);
5168         final double mzy = ma.getElementAt(2, 1);
5169 
5170         final Acceleration bx = new Acceleration(biasX,
5171                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5172         final Acceleration by = new Acceleration(biasY,
5173                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5174         final Acceleration bz = new Acceleration(biasZ,
5175                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5176 
5177         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5178                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
5179                         true, bx, by, bz, sx, sy, sz,
5180                         mxy, mxz, myx, myz, mzx, mzy, this);
5181 
5182         // check default values
5183         assertEquals(calibrator.getBiasX(), biasX, 0.0);
5184         assertEquals(calibrator.getBiasY(), biasY, 0.0);
5185         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5186         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5187         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5188         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5189         final Acceleration bx2 = new Acceleration(0.0,
5190                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5191         calibrator.getBiasXAsAcceleration(bx2);
5192         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5193         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5194         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5195         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5196         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5197         final Acceleration by2 = new Acceleration(0.0,
5198                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5199         calibrator.getBiasYAsAcceleration(by2);
5200         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5201         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5202         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5203         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5204         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5205         final Acceleration bz2 = new Acceleration(0.0,
5206                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5207         calibrator.getBiasZAsAcceleration(bz2);
5208         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5209         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5210         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5211         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5212         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5213         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5214         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5215         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5216         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5217         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5218         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5219         final double[] bias1 = calibrator.getBias();
5220         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5221         final double[] bias2 = new double[3];
5222         calibrator.getBias(bias2);
5223         assertArrayEquals(bias1, bias2, 0.0);
5224         final Matrix b1 = calibrator.getBiasAsMatrix();
5225         assertEquals(b1, ba);
5226         final Matrix b2 = new Matrix(3, 1);
5227         calibrator.getBiasAsMatrix(b2);
5228         assertEquals(b1, b2);
5229         final Matrix ma1 = new Matrix(3, 3);
5230         ma1.setSubmatrix(0, 0,
5231                 2, 2,
5232                 new double[]{sx, myx, mzx,
5233                         mxy, sy, mzy,
5234                         mxz, myz, sz});
5235         assertEquals(calibrator.getInitialMa(), ma1);
5236         final Matrix ma2 = new Matrix(3, 3);
5237         calibrator.getInitialMa(ma2);
5238         assertEquals(ma1, ma2);
5239         assertSame(calibrator.getMeasurements(), measurements);
5240         assertTrue(calibrator.isCommonAxisUsed());
5241         assertSame(calibrator.getListener(), this);
5242         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
5243         assertFalse(calibrator.isReady());
5244         assertFalse(calibrator.isRunning());
5245         assertNull(calibrator.getEstimatedMa());
5246         assertNull(calibrator.getEstimatedSx());
5247         assertNull(calibrator.getEstimatedSy());
5248         assertNull(calibrator.getEstimatedSz());
5249         assertNull(calibrator.getEstimatedMxy());
5250         assertNull(calibrator.getEstimatedMxz());
5251         assertNull(calibrator.getEstimatedMyx());
5252         assertNull(calibrator.getEstimatedMyz());
5253         assertNull(calibrator.getEstimatedMzx());
5254         assertNull(calibrator.getEstimatedMzy());
5255         assertNull(calibrator.getEstimatedCovariance());
5256         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5257         assertNull(calibrator.getGroundTruthGravityNorm());
5258         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5259         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5260     }
5261 
5262     @Test
5263     public void testConstructor55() throws WrongSizeException {
5264         final Matrix ba = generateBa();
5265         final double[] bias = ba.getBuffer();
5266         final double biasX = ba.getElementAtIndex(0);
5267         final double biasY = ba.getElementAtIndex(1);
5268         final double biasZ = ba.getElementAtIndex(2);
5269 
5270         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5271                 new KnownBiasAndGravityNormAccelerometerCalibrator(bias);
5272 
5273         // check default values
5274         assertEquals(calibrator.getBiasX(), biasX, 0.0);
5275         assertEquals(calibrator.getBiasY(), biasY, 0.0);
5276         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5277         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5278         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5279         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5280         final Acceleration bx2 = new Acceleration(0.0,
5281                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5282         calibrator.getBiasXAsAcceleration(bx2);
5283         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5284         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5285         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5286         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5287         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5288         final Acceleration by2 = new Acceleration(0.0,
5289                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5290         calibrator.getBiasYAsAcceleration(by2);
5291         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5292         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5293         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5294         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5295         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5296         final Acceleration bz2 = new Acceleration(0.0,
5297                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5298         calibrator.getBiasZAsAcceleration(bz2);
5299         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5300         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5301         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5302         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5303         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5304         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5305         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5306         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5307         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5308         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
5309         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
5310         final double[] bias1 = calibrator.getBias();
5311         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5312         final double[] bias2 = new double[3];
5313         calibrator.getBias(bias2);
5314         assertArrayEquals(bias1, bias2, 0.0);
5315         final Matrix b1 = calibrator.getBiasAsMatrix();
5316         assertEquals(b1, ba);
5317         final Matrix b2 = new Matrix(3, 1);
5318         calibrator.getBiasAsMatrix(b2);
5319         assertEquals(b1, b2);
5320         final Matrix ma1 = calibrator.getInitialMa();
5321         assertEquals(ma1, new Matrix(3, 3));
5322         final Matrix ma2 = new Matrix(3, 3);
5323         calibrator.getInitialMa(ma2);
5324         assertEquals(ma1, ma2);
5325         assertNull(calibrator.getMeasurements());
5326         assertFalse(calibrator.isCommonAxisUsed());
5327         assertNull(calibrator.getListener());
5328         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5329         assertFalse(calibrator.isReady());
5330         assertFalse(calibrator.isRunning());
5331         assertNull(calibrator.getEstimatedMa());
5332         assertNull(calibrator.getEstimatedSx());
5333         assertNull(calibrator.getEstimatedSy());
5334         assertNull(calibrator.getEstimatedSz());
5335         assertNull(calibrator.getEstimatedMxy());
5336         assertNull(calibrator.getEstimatedMxz());
5337         assertNull(calibrator.getEstimatedMyx());
5338         assertNull(calibrator.getEstimatedMyz());
5339         assertNull(calibrator.getEstimatedMzx());
5340         assertNull(calibrator.getEstimatedMzy());
5341         assertNull(calibrator.getEstimatedCovariance());
5342         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5343         assertNull(calibrator.getGroundTruthGravityNorm());
5344         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5345         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5346 
5347         // Force IllegalArgumentException
5348         calibrator = null;
5349         try {
5350             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
5351                     new double[1]);
5352             fail("IllegalArgumentException expected but not thrown");
5353         } catch (final IllegalArgumentException ignore) {
5354         }
5355         assertNull(calibrator);
5356     }
5357 
5358     @Test
5359     public void testConstructor56() throws WrongSizeException {
5360         final Matrix ba = generateBa();
5361         final double[] bias = ba.getBuffer();
5362         final double biasX = ba.getElementAtIndex(0);
5363         final double biasY = ba.getElementAtIndex(1);
5364         final double biasZ = ba.getElementAtIndex(2);
5365 
5366         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5367                 new KnownBiasAndGravityNormAccelerometerCalibrator(bias,
5368                         this);
5369 
5370         // check default values
5371         assertEquals(calibrator.getBiasX(), biasX, 0.0);
5372         assertEquals(calibrator.getBiasY(), biasY, 0.0);
5373         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5374         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5375         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5376         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5377         final Acceleration bx2 = new Acceleration(0.0,
5378                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5379         calibrator.getBiasXAsAcceleration(bx2);
5380         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5381         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5382         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5383         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5384         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5385         final Acceleration by2 = new Acceleration(0.0,
5386                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5387         calibrator.getBiasYAsAcceleration(by2);
5388         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5389         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5390         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5391         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5392         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5393         final Acceleration bz2 = new Acceleration(0.0,
5394                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5395         calibrator.getBiasZAsAcceleration(bz2);
5396         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5397         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5398         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5399         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5400         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5401         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5402         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5403         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5404         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5405         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
5406         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
5407         final double[] bias1 = calibrator.getBias();
5408         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5409         final double[] bias2 = new double[3];
5410         calibrator.getBias(bias2);
5411         assertArrayEquals(bias1, bias2, 0.0);
5412         final Matrix b1 = calibrator.getBiasAsMatrix();
5413         assertEquals(b1, ba);
5414         final Matrix b2 = new Matrix(3, 1);
5415         calibrator.getBiasAsMatrix(b2);
5416         assertEquals(b1, b2);
5417         final Matrix ma1 = calibrator.getInitialMa();
5418         assertEquals(ma1, new Matrix(3, 3));
5419         final Matrix ma2 = new Matrix(3, 3);
5420         calibrator.getInitialMa(ma2);
5421         assertEquals(ma1, ma2);
5422         assertNull(calibrator.getMeasurements());
5423         assertFalse(calibrator.isCommonAxisUsed());
5424         assertSame(calibrator.getListener(), this);
5425         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5426         assertFalse(calibrator.isReady());
5427         assertFalse(calibrator.isRunning());
5428         assertNull(calibrator.getEstimatedMa());
5429         assertNull(calibrator.getEstimatedSx());
5430         assertNull(calibrator.getEstimatedSy());
5431         assertNull(calibrator.getEstimatedSz());
5432         assertNull(calibrator.getEstimatedMxy());
5433         assertNull(calibrator.getEstimatedMxz());
5434         assertNull(calibrator.getEstimatedMyx());
5435         assertNull(calibrator.getEstimatedMyz());
5436         assertNull(calibrator.getEstimatedMzx());
5437         assertNull(calibrator.getEstimatedMzy());
5438         assertNull(calibrator.getEstimatedCovariance());
5439         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5440         assertNull(calibrator.getGroundTruthGravityNorm());
5441         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5442         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5443 
5444         // Force IllegalArgumentException
5445         calibrator = null;
5446         try {
5447             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
5448                     new double[1], this);
5449             fail("IllegalArgumentException expected but not thrown");
5450         } catch (final IllegalArgumentException ignore) {
5451         }
5452         assertNull(calibrator);
5453     }
5454 
5455     @Test
5456     public void testConstructor57() throws WrongSizeException {
5457         final Collection<StandardDeviationBodyKinematics> measurements =
5458                 Collections.emptyList();
5459 
5460         final Matrix ba = generateBa();
5461         final double[] bias = ba.getBuffer();
5462         final double biasX = ba.getElementAtIndex(0);
5463         final double biasY = ba.getElementAtIndex(1);
5464         final double biasZ = ba.getElementAtIndex(2);
5465 
5466         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5467                 new KnownBiasAndGravityNormAccelerometerCalibrator(
5468                         measurements, bias);
5469 
5470         // check default values
5471         assertEquals(calibrator.getBiasX(), biasX, 0.0);
5472         assertEquals(calibrator.getBiasY(), biasY, 0.0);
5473         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5474         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5475         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5476         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5477         final Acceleration bx2 = new Acceleration(0.0,
5478                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5479         calibrator.getBiasXAsAcceleration(bx2);
5480         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5481         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5482         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5483         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5484         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5485         final Acceleration by2 = new Acceleration(0.0,
5486                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5487         calibrator.getBiasYAsAcceleration(by2);
5488         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5489         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5490         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5491         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5492         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5493         final Acceleration bz2 = new Acceleration(0.0,
5494                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5495         calibrator.getBiasZAsAcceleration(bz2);
5496         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5497         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5498         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5499         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5500         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5501         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5502         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5503         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5504         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5505         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
5506         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
5507         final double[] bias1 = calibrator.getBias();
5508         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5509         final double[] bias2 = new double[3];
5510         calibrator.getBias(bias2);
5511         assertArrayEquals(bias1, bias2, 0.0);
5512         final Matrix b1 = calibrator.getBiasAsMatrix();
5513         assertEquals(b1, ba);
5514         final Matrix b2 = new Matrix(3, 1);
5515         calibrator.getBiasAsMatrix(b2);
5516         assertEquals(b1, b2);
5517         final Matrix ma1 = calibrator.getInitialMa();
5518         assertEquals(ma1, new Matrix(3, 3));
5519         final Matrix ma2 = new Matrix(3, 3);
5520         calibrator.getInitialMa(ma2);
5521         assertEquals(ma1, ma2);
5522         assertSame(calibrator.getMeasurements(), measurements);
5523         assertFalse(calibrator.isCommonAxisUsed());
5524         assertNull(calibrator.getListener());
5525         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5526         assertFalse(calibrator.isReady());
5527         assertFalse(calibrator.isRunning());
5528         assertNull(calibrator.getEstimatedMa());
5529         assertNull(calibrator.getEstimatedSx());
5530         assertNull(calibrator.getEstimatedSy());
5531         assertNull(calibrator.getEstimatedSz());
5532         assertNull(calibrator.getEstimatedMxy());
5533         assertNull(calibrator.getEstimatedMxz());
5534         assertNull(calibrator.getEstimatedMyx());
5535         assertNull(calibrator.getEstimatedMyz());
5536         assertNull(calibrator.getEstimatedMzx());
5537         assertNull(calibrator.getEstimatedMzy());
5538         assertNull(calibrator.getEstimatedCovariance());
5539         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5540         assertNull(calibrator.getGroundTruthGravityNorm());
5541         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5542         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5543 
5544         // Force IllegalArgumentException
5545         calibrator = null;
5546         try {
5547             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
5548                     measurements, new double[1]);
5549             fail("IllegalArgumentException expected but not thrown");
5550         } catch (final IllegalArgumentException ignore) {
5551         }
5552         assertNull(calibrator);
5553     }
5554 
5555     @Test
5556     public void testConstructor58() throws WrongSizeException {
5557         final Collection<StandardDeviationBodyKinematics> measurements =
5558                 Collections.emptyList();
5559 
5560         final Matrix ba = generateBa();
5561         final double[] bias = ba.getBuffer();
5562         final double biasX = ba.getElementAtIndex(0);
5563         final double biasY = ba.getElementAtIndex(1);
5564         final double biasZ = ba.getElementAtIndex(2);
5565 
5566         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5567                 new KnownBiasAndGravityNormAccelerometerCalibrator(
5568                         measurements, bias, this);
5569 
5570         // check default values
5571         assertEquals(calibrator.getBiasX(), biasX, 0.0);
5572         assertEquals(calibrator.getBiasY(), biasY, 0.0);
5573         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5574         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5575         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5576         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5577         final Acceleration bx2 = new Acceleration(0.0,
5578                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5579         calibrator.getBiasXAsAcceleration(bx2);
5580         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5581         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5582         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5583         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5584         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5585         final Acceleration by2 = new Acceleration(0.0,
5586                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5587         calibrator.getBiasYAsAcceleration(by2);
5588         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5589         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5590         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5591         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5592         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5593         final Acceleration bz2 = new Acceleration(0.0,
5594                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5595         calibrator.getBiasZAsAcceleration(bz2);
5596         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5597         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5598         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5599         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5600         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5601         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5602         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5603         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5604         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5605         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
5606         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
5607         final double[] bias1 = calibrator.getBias();
5608         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5609         final double[] bias2 = new double[3];
5610         calibrator.getBias(bias2);
5611         assertArrayEquals(bias1, bias2, 0.0);
5612         final Matrix b1 = calibrator.getBiasAsMatrix();
5613         assertEquals(b1, ba);
5614         final Matrix b2 = new Matrix(3, 1);
5615         calibrator.getBiasAsMatrix(b2);
5616         assertEquals(b1, b2);
5617         final Matrix ma1 = calibrator.getInitialMa();
5618         assertEquals(ma1, new Matrix(3, 3));
5619         final Matrix ma2 = new Matrix(3, 3);
5620         calibrator.getInitialMa(ma2);
5621         assertEquals(ma1, ma2);
5622         assertSame(calibrator.getMeasurements(), measurements);
5623         assertFalse(calibrator.isCommonAxisUsed());
5624         assertSame(calibrator.getListener(), this);
5625         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5626         assertFalse(calibrator.isReady());
5627         assertFalse(calibrator.isRunning());
5628         assertNull(calibrator.getEstimatedMa());
5629         assertNull(calibrator.getEstimatedSx());
5630         assertNull(calibrator.getEstimatedSy());
5631         assertNull(calibrator.getEstimatedSz());
5632         assertNull(calibrator.getEstimatedMxy());
5633         assertNull(calibrator.getEstimatedMxz());
5634         assertNull(calibrator.getEstimatedMyx());
5635         assertNull(calibrator.getEstimatedMyz());
5636         assertNull(calibrator.getEstimatedMzx());
5637         assertNull(calibrator.getEstimatedMzy());
5638         assertNull(calibrator.getEstimatedCovariance());
5639         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5640         assertNull(calibrator.getGroundTruthGravityNorm());
5641         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5642         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5643 
5644         // Force IllegalArgumentException
5645         calibrator = null;
5646         try {
5647             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
5648                     measurements, new double[1], this);
5649             fail("IllegalArgumentException expected but not thrown");
5650         } catch (final IllegalArgumentException ignore) {
5651         }
5652         assertNull(calibrator);
5653     }
5654 
5655     @Test
5656     public void testConstructor59() throws WrongSizeException {
5657         final Matrix ba = generateBa();
5658         final double[] bias = ba.getBuffer();
5659         final double biasX = ba.getElementAtIndex(0);
5660         final double biasY = ba.getElementAtIndex(1);
5661         final double biasZ = ba.getElementAtIndex(2);
5662 
5663         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5664                 new KnownBiasAndGravityNormAccelerometerCalibrator(
5665                         true, bias);
5666 
5667         // check default values
5668         assertEquals(calibrator.getBiasX(), biasX, 0.0);
5669         assertEquals(calibrator.getBiasY(), biasY, 0.0);
5670         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5671         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5672         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5673         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5674         final Acceleration bx2 = new Acceleration(0.0,
5675                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5676         calibrator.getBiasXAsAcceleration(bx2);
5677         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5678         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5679         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5680         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5681         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5682         final Acceleration by2 = new Acceleration(0.0,
5683                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5684         calibrator.getBiasYAsAcceleration(by2);
5685         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5686         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5687         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5688         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5689         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5690         final Acceleration bz2 = new Acceleration(0.0,
5691                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5692         calibrator.getBiasZAsAcceleration(bz2);
5693         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5694         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5695         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5696         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5697         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5698         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5699         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5700         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5701         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5702         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
5703         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
5704         final double[] bias1 = calibrator.getBias();
5705         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5706         final double[] bias2 = new double[3];
5707         calibrator.getBias(bias2);
5708         assertArrayEquals(bias1, bias2, 0.0);
5709         final Matrix b1 = calibrator.getBiasAsMatrix();
5710         assertEquals(b1, ba);
5711         final Matrix b2 = new Matrix(3, 1);
5712         calibrator.getBiasAsMatrix(b2);
5713         assertEquals(b1, b2);
5714         final Matrix ma1 = calibrator.getInitialMa();
5715         assertEquals(ma1, new Matrix(3, 3));
5716         final Matrix ma2 = new Matrix(3, 3);
5717         calibrator.getInitialMa(ma2);
5718         assertEquals(ma1, ma2);
5719         assertNull(calibrator.getMeasurements());
5720         assertTrue(calibrator.isCommonAxisUsed());
5721         assertNull(calibrator.getListener());
5722         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
5723         assertFalse(calibrator.isReady());
5724         assertFalse(calibrator.isRunning());
5725         assertNull(calibrator.getEstimatedMa());
5726         assertNull(calibrator.getEstimatedSx());
5727         assertNull(calibrator.getEstimatedSy());
5728         assertNull(calibrator.getEstimatedSz());
5729         assertNull(calibrator.getEstimatedMxy());
5730         assertNull(calibrator.getEstimatedMxz());
5731         assertNull(calibrator.getEstimatedMyx());
5732         assertNull(calibrator.getEstimatedMyz());
5733         assertNull(calibrator.getEstimatedMzx());
5734         assertNull(calibrator.getEstimatedMzy());
5735         assertNull(calibrator.getEstimatedCovariance());
5736         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5737         assertNull(calibrator.getGroundTruthGravityNorm());
5738         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5739         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5740 
5741         // Force IllegalArgumentException
5742         calibrator = null;
5743         try {
5744             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
5745                     true, new double[1]);
5746             fail("IllegalArgumentException expected but not thrown");
5747         } catch (final IllegalArgumentException ignore) {
5748         }
5749         assertNull(calibrator);
5750     }
5751 
5752     @Test
5753     public void testConstructor60() throws WrongSizeException {
5754         final Matrix ba = generateBa();
5755         final double[] bias = ba.getBuffer();
5756         final double biasX = ba.getElementAtIndex(0);
5757         final double biasY = ba.getElementAtIndex(1);
5758         final double biasZ = ba.getElementAtIndex(2);
5759 
5760         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5761                 new KnownBiasAndGravityNormAccelerometerCalibrator(
5762                         true, bias, this);
5763 
5764         // check default values
5765         assertEquals(calibrator.getBiasX(), biasX, 0.0);
5766         assertEquals(calibrator.getBiasY(), biasY, 0.0);
5767         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5768         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5769         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5770         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5771         final Acceleration bx2 = new Acceleration(0.0,
5772                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5773         calibrator.getBiasXAsAcceleration(bx2);
5774         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5775         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5776         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5777         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5778         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5779         final Acceleration by2 = new Acceleration(0.0,
5780                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5781         calibrator.getBiasYAsAcceleration(by2);
5782         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5783         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5784         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5785         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5786         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5787         final Acceleration bz2 = new Acceleration(0.0,
5788                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5789         calibrator.getBiasZAsAcceleration(bz2);
5790         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5791         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5792         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5793         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5794         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5795         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5796         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5797         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5798         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5799         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
5800         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
5801         final double[] bias1 = calibrator.getBias();
5802         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5803         final double[] bias2 = new double[3];
5804         calibrator.getBias(bias2);
5805         assertArrayEquals(bias1, bias2, 0.0);
5806         final Matrix b1 = calibrator.getBiasAsMatrix();
5807         assertEquals(b1, ba);
5808         final Matrix b2 = new Matrix(3, 1);
5809         calibrator.getBiasAsMatrix(b2);
5810         assertEquals(b1, b2);
5811         final Matrix ma1 = calibrator.getInitialMa();
5812         assertEquals(ma1, new Matrix(3, 3));
5813         final Matrix ma2 = new Matrix(3, 3);
5814         calibrator.getInitialMa(ma2);
5815         assertEquals(ma1, ma2);
5816         assertNull(calibrator.getMeasurements());
5817         assertTrue(calibrator.isCommonAxisUsed());
5818         assertSame(calibrator.getListener(), this);
5819         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
5820         assertFalse(calibrator.isReady());
5821         assertFalse(calibrator.isRunning());
5822         assertNull(calibrator.getEstimatedMa());
5823         assertNull(calibrator.getEstimatedSx());
5824         assertNull(calibrator.getEstimatedSy());
5825         assertNull(calibrator.getEstimatedSz());
5826         assertNull(calibrator.getEstimatedMxy());
5827         assertNull(calibrator.getEstimatedMxz());
5828         assertNull(calibrator.getEstimatedMyx());
5829         assertNull(calibrator.getEstimatedMyz());
5830         assertNull(calibrator.getEstimatedMzx());
5831         assertNull(calibrator.getEstimatedMzy());
5832         assertNull(calibrator.getEstimatedCovariance());
5833         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5834         assertNull(calibrator.getGroundTruthGravityNorm());
5835         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5836         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5837 
5838         // Force IllegalArgumentException
5839         calibrator = null;
5840         try {
5841             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
5842                     true, new double[1], this);
5843             fail("IllegalArgumentException expected but not thrown");
5844         } catch (final IllegalArgumentException ignore) {
5845         }
5846         assertNull(calibrator);
5847     }
5848 
5849     @Test
5850     public void testConstructor61() throws WrongSizeException {
5851         final Collection<StandardDeviationBodyKinematics> measurements =
5852                 Collections.emptyList();
5853 
5854         final Matrix ba = generateBa();
5855         final double[] bias = ba.getBuffer();
5856         final double biasX = ba.getElementAtIndex(0);
5857         final double biasY = ba.getElementAtIndex(1);
5858         final double biasZ = ba.getElementAtIndex(2);
5859 
5860         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5861                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
5862                         true, bias);
5863 
5864         // check default values
5865         assertEquals(calibrator.getBiasX(), biasX, 0.0);
5866         assertEquals(calibrator.getBiasY(), biasY, 0.0);
5867         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5868         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5869         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5870         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5871         final Acceleration bx2 = new Acceleration(0.0,
5872                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5873         calibrator.getBiasXAsAcceleration(bx2);
5874         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5875         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5876         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5877         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5878         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5879         final Acceleration by2 = new Acceleration(0.0,
5880                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5881         calibrator.getBiasYAsAcceleration(by2);
5882         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5883         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5884         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5885         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5886         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5887         final Acceleration bz2 = new Acceleration(0.0,
5888                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5889         calibrator.getBiasZAsAcceleration(bz2);
5890         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5891         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5892         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5893         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5894         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5895         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5896         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5897         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5898         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5899         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
5900         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
5901         final double[] bias1 = calibrator.getBias();
5902         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5903         final double[] bias2 = new double[3];
5904         calibrator.getBias(bias2);
5905         assertArrayEquals(bias1, bias2, 0.0);
5906         final Matrix b1 = calibrator.getBiasAsMatrix();
5907         assertEquals(b1, ba);
5908         final Matrix b2 = new Matrix(3, 1);
5909         calibrator.getBiasAsMatrix(b2);
5910         assertEquals(b1, b2);
5911         final Matrix ma1 = calibrator.getInitialMa();
5912         assertEquals(ma1, new Matrix(3, 3));
5913         final Matrix ma2 = new Matrix(3, 3);
5914         calibrator.getInitialMa(ma2);
5915         assertEquals(ma1, ma2);
5916         assertSame(calibrator.getMeasurements(), measurements);
5917         assertTrue(calibrator.isCommonAxisUsed());
5918         assertNull(calibrator.getListener());
5919         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
5920         assertFalse(calibrator.isReady());
5921         assertFalse(calibrator.isRunning());
5922         assertNull(calibrator.getEstimatedMa());
5923         assertNull(calibrator.getEstimatedSx());
5924         assertNull(calibrator.getEstimatedSy());
5925         assertNull(calibrator.getEstimatedSz());
5926         assertNull(calibrator.getEstimatedMxy());
5927         assertNull(calibrator.getEstimatedMxz());
5928         assertNull(calibrator.getEstimatedMyx());
5929         assertNull(calibrator.getEstimatedMyz());
5930         assertNull(calibrator.getEstimatedMzx());
5931         assertNull(calibrator.getEstimatedMzy());
5932         assertNull(calibrator.getEstimatedCovariance());
5933         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5934         assertNull(calibrator.getGroundTruthGravityNorm());
5935         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5936         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5937 
5938         // Force IllegalArgumentException
5939         calibrator = null;
5940         try {
5941             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
5942                     measurements, true, new double[1]);
5943             fail("IllegalArgumentException expected but not thrown");
5944         } catch (final IllegalArgumentException ignore) {
5945         }
5946         assertNull(calibrator);
5947     }
5948 
5949     @Test
5950     public void testConstructor62() throws WrongSizeException {
5951         final Collection<StandardDeviationBodyKinematics> measurements =
5952                 Collections.emptyList();
5953 
5954         final Matrix ba = generateBa();
5955         final double[] bias = ba.getBuffer();
5956         final double biasX = ba.getElementAtIndex(0);
5957         final double biasY = ba.getElementAtIndex(1);
5958         final double biasZ = ba.getElementAtIndex(2);
5959 
5960         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
5961                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
5962                         true, bias, this);
5963 
5964         // check default values
5965         assertEquals(calibrator.getBiasX(), biasX, 0.0);
5966         assertEquals(calibrator.getBiasY(), biasY, 0.0);
5967         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
5968         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
5969         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5970         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5971         final Acceleration bx2 = new Acceleration(0.0,
5972                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5973         calibrator.getBiasXAsAcceleration(bx2);
5974         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5975         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5976         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
5977         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5978         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5979         final Acceleration by2 = new Acceleration(0.0,
5980                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5981         calibrator.getBiasYAsAcceleration(by2);
5982         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5983         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5984         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
5985         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5986         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5987         final Acceleration bz2 = new Acceleration(0.0,
5988                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5989         calibrator.getBiasZAsAcceleration(bz2);
5990         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5991         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5992         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5993         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5994         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5995         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5996         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5997         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5998         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5999         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6000         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6001         final double[] bias1 = calibrator.getBias();
6002         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6003         final double[] bias2 = new double[3];
6004         calibrator.getBias(bias2);
6005         assertArrayEquals(bias1, bias2, 0.0);
6006         final Matrix b1 = calibrator.getBiasAsMatrix();
6007         assertEquals(b1, ba);
6008         final Matrix b2 = new Matrix(3, 1);
6009         calibrator.getBiasAsMatrix(b2);
6010         assertEquals(b1, b2);
6011         final Matrix ma1 = calibrator.getInitialMa();
6012         assertEquals(ma1, new Matrix(3, 3));
6013         final Matrix ma2 = new Matrix(3, 3);
6014         calibrator.getInitialMa(ma2);
6015         assertEquals(ma1, ma2);
6016         assertSame(calibrator.getMeasurements(), measurements);
6017         assertTrue(calibrator.isCommonAxisUsed());
6018         assertSame(calibrator.getListener(), this);
6019         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
6020         assertFalse(calibrator.isReady());
6021         assertFalse(calibrator.isRunning());
6022         assertNull(calibrator.getEstimatedMa());
6023         assertNull(calibrator.getEstimatedSx());
6024         assertNull(calibrator.getEstimatedSy());
6025         assertNull(calibrator.getEstimatedSz());
6026         assertNull(calibrator.getEstimatedMxy());
6027         assertNull(calibrator.getEstimatedMxz());
6028         assertNull(calibrator.getEstimatedMyx());
6029         assertNull(calibrator.getEstimatedMyz());
6030         assertNull(calibrator.getEstimatedMzx());
6031         assertNull(calibrator.getEstimatedMzy());
6032         assertNull(calibrator.getEstimatedCovariance());
6033         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6034         assertNull(calibrator.getGroundTruthGravityNorm());
6035         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6036         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6037 
6038         // Force IllegalArgumentException
6039         calibrator = null;
6040         try {
6041             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6042                     measurements, true, new double[1],
6043                     this);
6044             fail("IllegalArgumentException expected but not thrown");
6045         } catch (final IllegalArgumentException ignore) {
6046         }
6047         assertNull(calibrator);
6048     }
6049 
6050     @Test
6051     public void testConstructor63() throws WrongSizeException {
6052         final Matrix ba = generateBa();
6053         final double biasX = ba.getElementAtIndex(0);
6054         final double biasY = ba.getElementAtIndex(1);
6055         final double biasZ = ba.getElementAtIndex(2);
6056 
6057         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
6058                 new KnownBiasAndGravityNormAccelerometerCalibrator(ba);
6059 
6060         // check default values
6061         assertEquals(calibrator.getBiasX(), biasX, 0.0);
6062         assertEquals(calibrator.getBiasY(), biasY, 0.0);
6063         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
6064         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
6065         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6066         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6067         final Acceleration bx2 = new Acceleration(0.0,
6068                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6069         calibrator.getBiasXAsAcceleration(bx2);
6070         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6071         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6072         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
6073         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6074         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6075         final Acceleration by2 = new Acceleration(0.0,
6076                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6077         calibrator.getBiasYAsAcceleration(by2);
6078         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6079         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6080         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
6081         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6082         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6083         final Acceleration bz2 = new Acceleration(0.0,
6084                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6085         calibrator.getBiasZAsAcceleration(bz2);
6086         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6087         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6088         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6089         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6090         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6091         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6092         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6093         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6094         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6095         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6096         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6097         final double[] bias1 = calibrator.getBias();
6098         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6099         final double[] bias2 = new double[3];
6100         calibrator.getBias(bias2);
6101         assertArrayEquals(bias1, bias2, 0.0);
6102         final Matrix b1 = calibrator.getBiasAsMatrix();
6103         assertEquals(b1, ba);
6104         final Matrix b2 = new Matrix(3, 1);
6105         calibrator.getBiasAsMatrix(b2);
6106         assertEquals(b1, b2);
6107         final Matrix ma1 = calibrator.getInitialMa();
6108         assertEquals(ma1, new Matrix(3, 3));
6109         final Matrix ma2 = new Matrix(3, 3);
6110         calibrator.getInitialMa(ma2);
6111         assertEquals(ma1, ma2);
6112         assertNull(calibrator.getMeasurements());
6113         assertFalse(calibrator.isCommonAxisUsed());
6114         assertNull(calibrator.getListener());
6115         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6116         assertFalse(calibrator.isReady());
6117         assertFalse(calibrator.isRunning());
6118         assertNull(calibrator.getEstimatedMa());
6119         assertNull(calibrator.getEstimatedSx());
6120         assertNull(calibrator.getEstimatedSy());
6121         assertNull(calibrator.getEstimatedSz());
6122         assertNull(calibrator.getEstimatedMxy());
6123         assertNull(calibrator.getEstimatedMxz());
6124         assertNull(calibrator.getEstimatedMyx());
6125         assertNull(calibrator.getEstimatedMyz());
6126         assertNull(calibrator.getEstimatedMzx());
6127         assertNull(calibrator.getEstimatedMzy());
6128         assertNull(calibrator.getEstimatedCovariance());
6129         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6130         assertNull(calibrator.getGroundTruthGravityNorm());
6131         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6132         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6133 
6134         // Force IllegalArgumentException
6135         calibrator = null;
6136         try {
6137             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6138                     new Matrix(1, 1));
6139             fail("IllegalArgumentException expected but not thrown");
6140         } catch (final IllegalArgumentException ignore) {
6141         }
6142         try {
6143             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6144                     new Matrix(1, 3));
6145             fail("IllegalArgumentException expected but not thrown");
6146         } catch (final IllegalArgumentException ignore) {
6147         }
6148         assertNull(calibrator);
6149     }
6150 
6151     @Test
6152     public void testConstructor64() throws WrongSizeException {
6153         final Matrix ba = generateBa();
6154         final double biasX = ba.getElementAtIndex(0);
6155         final double biasY = ba.getElementAtIndex(1);
6156         final double biasZ = ba.getElementAtIndex(2);
6157 
6158         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
6159                 new KnownBiasAndGravityNormAccelerometerCalibrator(
6160                         ba, this);
6161 
6162         // check default values
6163         assertEquals(calibrator.getBiasX(), biasX, 0.0);
6164         assertEquals(calibrator.getBiasY(), biasY, 0.0);
6165         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
6166         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
6167         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6168         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6169         final Acceleration bx2 = new Acceleration(0.0,
6170                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6171         calibrator.getBiasXAsAcceleration(bx2);
6172         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6173         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6174         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
6175         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6176         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6177         final Acceleration by2 = new Acceleration(0.0,
6178                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6179         calibrator.getBiasYAsAcceleration(by2);
6180         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6181         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6182         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
6183         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6184         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6185         final Acceleration bz2 = new Acceleration(0.0,
6186                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6187         calibrator.getBiasZAsAcceleration(bz2);
6188         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6189         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6190         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6191         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6192         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6193         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6194         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6195         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6196         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6197         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6198         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6199         final double[] bias1 = calibrator.getBias();
6200         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6201         final double[] bias2 = new double[3];
6202         calibrator.getBias(bias2);
6203         assertArrayEquals(bias1, bias2, 0.0);
6204         final Matrix b1 = calibrator.getBiasAsMatrix();
6205         assertEquals(b1, ba);
6206         final Matrix b2 = new Matrix(3, 1);
6207         calibrator.getBiasAsMatrix(b2);
6208         assertEquals(b1, b2);
6209         final Matrix ma1 = calibrator.getInitialMa();
6210         assertEquals(ma1, new Matrix(3, 3));
6211         final Matrix ma2 = new Matrix(3, 3);
6212         calibrator.getInitialMa(ma2);
6213         assertEquals(ma1, ma2);
6214         assertNull(calibrator.getMeasurements());
6215         assertFalse(calibrator.isCommonAxisUsed());
6216         assertSame(calibrator.getListener(), this);
6217         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6218         assertFalse(calibrator.isReady());
6219         assertFalse(calibrator.isRunning());
6220         assertNull(calibrator.getEstimatedMa());
6221         assertNull(calibrator.getEstimatedSx());
6222         assertNull(calibrator.getEstimatedSy());
6223         assertNull(calibrator.getEstimatedSz());
6224         assertNull(calibrator.getEstimatedMxy());
6225         assertNull(calibrator.getEstimatedMxz());
6226         assertNull(calibrator.getEstimatedMyx());
6227         assertNull(calibrator.getEstimatedMyz());
6228         assertNull(calibrator.getEstimatedMzx());
6229         assertNull(calibrator.getEstimatedMzy());
6230         assertNull(calibrator.getEstimatedCovariance());
6231         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6232         assertNull(calibrator.getGroundTruthGravityNorm());
6233         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6234         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6235 
6236         // Force IllegalArgumentException
6237         calibrator = null;
6238         try {
6239             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6240                     new Matrix(1, 1));
6241             fail("IllegalArgumentException expected but not thrown");
6242         } catch (final IllegalArgumentException ignore) {
6243         }
6244         try {
6245             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6246                     new Matrix(1, 3));
6247             fail("IllegalArgumentException expected but not thrown");
6248         } catch (final IllegalArgumentException ignore) {
6249         }
6250         assertNull(calibrator);
6251     }
6252 
6253     @Test
6254     public void testConstructor65() throws WrongSizeException {
6255         final Collection<StandardDeviationBodyKinematics> measurements =
6256                 Collections.emptyList();
6257 
6258         final Matrix ba = generateBa();
6259         final double biasX = ba.getElementAtIndex(0);
6260         final double biasY = ba.getElementAtIndex(1);
6261         final double biasZ = ba.getElementAtIndex(2);
6262 
6263         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
6264                 new KnownBiasAndGravityNormAccelerometerCalibrator(
6265                         measurements, ba);
6266 
6267         // check default values
6268         assertEquals(calibrator.getBiasX(), biasX, 0.0);
6269         assertEquals(calibrator.getBiasY(), biasY, 0.0);
6270         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
6271         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
6272         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6273         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6274         final Acceleration bx2 = new Acceleration(0.0,
6275                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6276         calibrator.getBiasXAsAcceleration(bx2);
6277         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6278         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6279         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
6280         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6281         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6282         final Acceleration by2 = new Acceleration(0.0,
6283                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6284         calibrator.getBiasYAsAcceleration(by2);
6285         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6286         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6287         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
6288         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6289         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6290         final Acceleration bz2 = new Acceleration(0.0,
6291                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6292         calibrator.getBiasZAsAcceleration(bz2);
6293         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6294         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6295         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6296         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6297         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6298         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6299         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6300         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6301         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6302         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6303         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6304         final double[] bias1 = calibrator.getBias();
6305         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6306         final double[] bias2 = new double[3];
6307         calibrator.getBias(bias2);
6308         assertArrayEquals(bias1, bias2, 0.0);
6309         final Matrix b1 = calibrator.getBiasAsMatrix();
6310         assertEquals(b1, ba);
6311         final Matrix b2 = new Matrix(3, 1);
6312         calibrator.getBiasAsMatrix(b2);
6313         assertEquals(b1, b2);
6314         final Matrix ma1 = calibrator.getInitialMa();
6315         assertEquals(ma1, new Matrix(3, 3));
6316         final Matrix ma2 = new Matrix(3, 3);
6317         calibrator.getInitialMa(ma2);
6318         assertEquals(ma1, ma2);
6319         assertSame(calibrator.getMeasurements(), measurements);
6320         assertFalse(calibrator.isCommonAxisUsed());
6321         assertNull(calibrator.getListener());
6322         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6323         assertFalse(calibrator.isReady());
6324         assertFalse(calibrator.isRunning());
6325         assertNull(calibrator.getEstimatedMa());
6326         assertNull(calibrator.getEstimatedSx());
6327         assertNull(calibrator.getEstimatedSy());
6328         assertNull(calibrator.getEstimatedSz());
6329         assertNull(calibrator.getEstimatedMxy());
6330         assertNull(calibrator.getEstimatedMxz());
6331         assertNull(calibrator.getEstimatedMyx());
6332         assertNull(calibrator.getEstimatedMyz());
6333         assertNull(calibrator.getEstimatedMzx());
6334         assertNull(calibrator.getEstimatedMzy());
6335         assertNull(calibrator.getEstimatedCovariance());
6336         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6337         assertNull(calibrator.getGroundTruthGravityNorm());
6338         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6339         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6340 
6341         // Force IllegalArgumentException
6342         calibrator = null;
6343         try {
6344             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
6345                     new Matrix(1, 1));
6346             fail("IllegalArgumentException expected but not thrown");
6347         } catch (final IllegalArgumentException ignore) {
6348         }
6349         try {
6350             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
6351                     new Matrix(1, 3));
6352             fail("IllegalArgumentException expected but not thrown");
6353         } catch (final IllegalArgumentException ignore) {
6354         }
6355         assertNull(calibrator);
6356     }
6357 
6358     @Test
6359     public void testConstructor66() throws WrongSizeException {
6360         final Collection<StandardDeviationBodyKinematics> measurements =
6361                 Collections.emptyList();
6362 
6363         final Matrix ba = generateBa();
6364         final double biasX = ba.getElementAtIndex(0);
6365         final double biasY = ba.getElementAtIndex(1);
6366         final double biasZ = ba.getElementAtIndex(2);
6367 
6368         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
6369                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, ba,
6370                         this);
6371 
6372         // check default values
6373         assertEquals(calibrator.getBiasX(), biasX, 0.0);
6374         assertEquals(calibrator.getBiasY(), biasY, 0.0);
6375         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
6376         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
6377         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6378         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6379         final Acceleration bx2 = new Acceleration(0.0,
6380                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6381         calibrator.getBiasXAsAcceleration(bx2);
6382         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6383         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6384         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
6385         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6386         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6387         final Acceleration by2 = new Acceleration(0.0,
6388                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6389         calibrator.getBiasYAsAcceleration(by2);
6390         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6391         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6392         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
6393         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6394         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6395         final Acceleration bz2 = new Acceleration(0.0,
6396                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6397         calibrator.getBiasZAsAcceleration(bz2);
6398         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6399         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6400         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6401         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6402         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6403         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6404         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6405         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6406         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6407         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6408         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6409         final double[] bias1 = calibrator.getBias();
6410         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6411         final double[] bias2 = new double[3];
6412         calibrator.getBias(bias2);
6413         assertArrayEquals(bias1, bias2, 0.0);
6414         final Matrix b1 = calibrator.getBiasAsMatrix();
6415         assertEquals(b1, ba);
6416         final Matrix b2 = new Matrix(3, 1);
6417         calibrator.getBiasAsMatrix(b2);
6418         assertEquals(b1, b2);
6419         final Matrix ma1 = calibrator.getInitialMa();
6420         assertEquals(ma1, new Matrix(3, 3));
6421         final Matrix ma2 = new Matrix(3, 3);
6422         calibrator.getInitialMa(ma2);
6423         assertEquals(ma1, ma2);
6424         assertSame(calibrator.getMeasurements(), measurements);
6425         assertFalse(calibrator.isCommonAxisUsed());
6426         assertSame(calibrator.getListener(), this);
6427         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6428         assertFalse(calibrator.isReady());
6429         assertFalse(calibrator.isRunning());
6430         assertNull(calibrator.getEstimatedMa());
6431         assertNull(calibrator.getEstimatedSx());
6432         assertNull(calibrator.getEstimatedSy());
6433         assertNull(calibrator.getEstimatedSz());
6434         assertNull(calibrator.getEstimatedMxy());
6435         assertNull(calibrator.getEstimatedMxz());
6436         assertNull(calibrator.getEstimatedMyx());
6437         assertNull(calibrator.getEstimatedMyz());
6438         assertNull(calibrator.getEstimatedMzx());
6439         assertNull(calibrator.getEstimatedMzy());
6440         assertNull(calibrator.getEstimatedCovariance());
6441         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6442         assertNull(calibrator.getGroundTruthGravityNorm());
6443         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6444         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6445 
6446         // Force IllegalArgumentException
6447         calibrator = null;
6448         try {
6449             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6450                     measurements, new Matrix(1, 1),
6451                     this);
6452             fail("IllegalArgumentException expected but not thrown");
6453         } catch (final IllegalArgumentException ignore) {
6454         }
6455         try {
6456             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6457                     measurements, new Matrix(1, 3),
6458                     this);
6459             fail("IllegalArgumentException expected but not thrown");
6460         } catch (final IllegalArgumentException ignore) {
6461         }
6462         assertNull(calibrator);
6463     }
6464 
6465     @Test
6466     public void testConstructor67() throws WrongSizeException {
6467         final Matrix ba = generateBa();
6468         final double biasX = ba.getElementAtIndex(0);
6469         final double biasY = ba.getElementAtIndex(1);
6470         final double biasZ = ba.getElementAtIndex(2);
6471 
6472         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
6473                 new KnownBiasAndGravityNormAccelerometerCalibrator(
6474                         true, ba);
6475 
6476         // check default values
6477         assertEquals(calibrator.getBiasX(), biasX, 0.0);
6478         assertEquals(calibrator.getBiasY(), biasY, 0.0);
6479         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
6480         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
6481         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6482         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6483         final Acceleration bx2 = new Acceleration(0.0,
6484                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6485         calibrator.getBiasXAsAcceleration(bx2);
6486         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6487         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6488         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
6489         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6490         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6491         final Acceleration by2 = new Acceleration(0.0,
6492                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6493         calibrator.getBiasYAsAcceleration(by2);
6494         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6495         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6496         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
6497         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6498         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6499         final Acceleration bz2 = new Acceleration(0.0,
6500                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6501         calibrator.getBiasZAsAcceleration(bz2);
6502         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6503         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6504         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6505         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6506         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6507         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6508         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6509         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6510         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6511         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6512         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6513         final double[] bias1 = calibrator.getBias();
6514         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6515         final double[] bias2 = new double[3];
6516         calibrator.getBias(bias2);
6517         assertArrayEquals(bias1, bias2, 0.0);
6518         final Matrix b1 = calibrator.getBiasAsMatrix();
6519         assertEquals(b1, ba);
6520         final Matrix b2 = new Matrix(3, 1);
6521         calibrator.getBiasAsMatrix(b2);
6522         assertEquals(b1, b2);
6523         final Matrix ma1 = calibrator.getInitialMa();
6524         assertEquals(ma1, new Matrix(3, 3));
6525         final Matrix ma2 = new Matrix(3, 3);
6526         calibrator.getInitialMa(ma2);
6527         assertEquals(ma1, ma2);
6528         assertNull(calibrator.getMeasurements());
6529         assertTrue(calibrator.isCommonAxisUsed());
6530         assertNull(calibrator.getListener());
6531         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
6532         assertFalse(calibrator.isReady());
6533         assertFalse(calibrator.isRunning());
6534         assertNull(calibrator.getEstimatedMa());
6535         assertNull(calibrator.getEstimatedSx());
6536         assertNull(calibrator.getEstimatedSy());
6537         assertNull(calibrator.getEstimatedSz());
6538         assertNull(calibrator.getEstimatedMxy());
6539         assertNull(calibrator.getEstimatedMxz());
6540         assertNull(calibrator.getEstimatedMyx());
6541         assertNull(calibrator.getEstimatedMyz());
6542         assertNull(calibrator.getEstimatedMzx());
6543         assertNull(calibrator.getEstimatedMzy());
6544         assertNull(calibrator.getEstimatedCovariance());
6545         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6546         assertNull(calibrator.getGroundTruthGravityNorm());
6547         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6548         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6549 
6550         // Force IllegalArgumentException
6551         calibrator = null;
6552         try {
6553             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6554                     true, new Matrix(1, 1));
6555             fail("IllegalArgumentException expected but not thrown");
6556         } catch (final IllegalArgumentException ignore) {
6557         }
6558         try {
6559             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6560                     true, new Matrix(1, 3));
6561             fail("IllegalArgumentException expected but not thrown");
6562         } catch (final IllegalArgumentException ignore) {
6563         }
6564         assertNull(calibrator);
6565     }
6566 
6567     @Test
6568     public void testConstructor68() throws WrongSizeException {
6569         final Matrix ba = generateBa();
6570         final double biasX = ba.getElementAtIndex(0);
6571         final double biasY = ba.getElementAtIndex(1);
6572         final double biasZ = ba.getElementAtIndex(2);
6573 
6574         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
6575                 new KnownBiasAndGravityNormAccelerometerCalibrator(
6576                         true, ba, this);
6577 
6578         // check default values
6579         assertEquals(calibrator.getBiasX(), biasX, 0.0);
6580         assertEquals(calibrator.getBiasY(), biasY, 0.0);
6581         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
6582         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
6583         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6584         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6585         final Acceleration bx2 = new Acceleration(0.0,
6586                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6587         calibrator.getBiasXAsAcceleration(bx2);
6588         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6589         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6590         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
6591         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6592         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6593         final Acceleration by2 = new Acceleration(0.0,
6594                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6595         calibrator.getBiasYAsAcceleration(by2);
6596         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6597         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6598         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
6599         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6600         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6601         final Acceleration bz2 = new Acceleration(0.0,
6602                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6603         calibrator.getBiasZAsAcceleration(bz2);
6604         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6605         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6606         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6607         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6608         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6609         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6610         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6611         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6612         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6613         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6614         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6615         final double[] bias1 = calibrator.getBias();
6616         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6617         final double[] bias2 = new double[3];
6618         calibrator.getBias(bias2);
6619         assertArrayEquals(bias1, bias2, 0.0);
6620         final Matrix b1 = calibrator.getBiasAsMatrix();
6621         assertEquals(b1, ba);
6622         final Matrix b2 = new Matrix(3, 1);
6623         calibrator.getBiasAsMatrix(b2);
6624         assertEquals(b1, b2);
6625         final Matrix ma1 = calibrator.getInitialMa();
6626         assertEquals(ma1, new Matrix(3, 3));
6627         final Matrix ma2 = new Matrix(3, 3);
6628         calibrator.getInitialMa(ma2);
6629         assertEquals(ma1, ma2);
6630         assertNull(calibrator.getMeasurements());
6631         assertTrue(calibrator.isCommonAxisUsed());
6632         assertSame(calibrator.getListener(), this);
6633         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
6634         assertFalse(calibrator.isReady());
6635         assertFalse(calibrator.isRunning());
6636         assertNull(calibrator.getEstimatedMa());
6637         assertNull(calibrator.getEstimatedSx());
6638         assertNull(calibrator.getEstimatedSy());
6639         assertNull(calibrator.getEstimatedSz());
6640         assertNull(calibrator.getEstimatedMxy());
6641         assertNull(calibrator.getEstimatedMxz());
6642         assertNull(calibrator.getEstimatedMyx());
6643         assertNull(calibrator.getEstimatedMyz());
6644         assertNull(calibrator.getEstimatedMzx());
6645         assertNull(calibrator.getEstimatedMzy());
6646         assertNull(calibrator.getEstimatedCovariance());
6647         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6648         assertNull(calibrator.getGroundTruthGravityNorm());
6649         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6650         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6651 
6652         // Force IllegalArgumentException
6653         calibrator = null;
6654         try {
6655             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6656                     true, new Matrix(1, 1),
6657                     this);
6658             fail("IllegalArgumentException expected but not thrown");
6659         } catch (final IllegalArgumentException ignore) {
6660         }
6661         try {
6662             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6663                     true, new Matrix(1, 3),
6664                     this);
6665             fail("IllegalArgumentException expected but not thrown");
6666         } catch (final IllegalArgumentException ignore) {
6667         }
6668         assertNull(calibrator);
6669     }
6670 
6671     @Test
6672     public void testConstructor69() throws WrongSizeException {
6673         final Collection<StandardDeviationBodyKinematics> measurements =
6674                 Collections.emptyList();
6675 
6676         final Matrix ba = generateBa();
6677         final double biasX = ba.getElementAtIndex(0);
6678         final double biasY = ba.getElementAtIndex(1);
6679         final double biasZ = ba.getElementAtIndex(2);
6680 
6681         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
6682                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
6683                         true, ba);
6684 
6685         // check default values
6686         assertEquals(calibrator.getBiasX(), biasX, 0.0);
6687         assertEquals(calibrator.getBiasY(), biasY, 0.0);
6688         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
6689         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
6690         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6691         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6692         final Acceleration bx2 = new Acceleration(0.0,
6693                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6694         calibrator.getBiasXAsAcceleration(bx2);
6695         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6696         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6697         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
6698         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6699         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6700         final Acceleration by2 = new Acceleration(0.0,
6701                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6702         calibrator.getBiasYAsAcceleration(by2);
6703         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6704         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6705         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
6706         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6707         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6708         final Acceleration bz2 = new Acceleration(0.0,
6709                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6710         calibrator.getBiasZAsAcceleration(bz2);
6711         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6712         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6713         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6714         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6715         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6716         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6717         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6718         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6719         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6720         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6721         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6722         final double[] bias1 = calibrator.getBias();
6723         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6724         final double[] bias2 = new double[3];
6725         calibrator.getBias(bias2);
6726         assertArrayEquals(bias1, bias2, 0.0);
6727         final Matrix b1 = calibrator.getBiasAsMatrix();
6728         assertEquals(b1, ba);
6729         final Matrix b2 = new Matrix(3, 1);
6730         calibrator.getBiasAsMatrix(b2);
6731         assertEquals(b1, b2);
6732         final Matrix ma1 = calibrator.getInitialMa();
6733         assertEquals(ma1, new Matrix(3, 3));
6734         final Matrix ma2 = new Matrix(3, 3);
6735         calibrator.getInitialMa(ma2);
6736         assertEquals(ma1, ma2);
6737         assertSame(calibrator.getMeasurements(), measurements);
6738         assertTrue(calibrator.isCommonAxisUsed());
6739         assertNull(calibrator.getListener());
6740         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
6741         assertFalse(calibrator.isReady());
6742         assertFalse(calibrator.isRunning());
6743         assertNull(calibrator.getEstimatedMa());
6744         assertNull(calibrator.getEstimatedSx());
6745         assertNull(calibrator.getEstimatedSy());
6746         assertNull(calibrator.getEstimatedSz());
6747         assertNull(calibrator.getEstimatedMxy());
6748         assertNull(calibrator.getEstimatedMxz());
6749         assertNull(calibrator.getEstimatedMyx());
6750         assertNull(calibrator.getEstimatedMyz());
6751         assertNull(calibrator.getEstimatedMzx());
6752         assertNull(calibrator.getEstimatedMzy());
6753         assertNull(calibrator.getEstimatedCovariance());
6754         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6755         assertNull(calibrator.getGroundTruthGravityNorm());
6756         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6757         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6758 
6759         // Force IllegalArgumentException
6760         calibrator = null;
6761         try {
6762             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6763                     measurements, true,
6764                     new Matrix(1, 1));
6765             fail("IllegalArgumentException expected but not thrown");
6766         } catch (final IllegalArgumentException ignore) {
6767         }
6768         try {
6769             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6770                     measurements, true,
6771                     new Matrix(1, 3));
6772             fail("IllegalArgumentException expected but not thrown");
6773         } catch (final IllegalArgumentException ignore) {
6774         }
6775         assertNull(calibrator);
6776     }
6777 
6778     @Test
6779     public void testConstructor70() throws WrongSizeException {
6780         final Collection<StandardDeviationBodyKinematics> measurements =
6781                 Collections.emptyList();
6782 
6783         final Matrix ba = generateBa();
6784         final double biasX = ba.getElementAtIndex(0);
6785         final double biasY = ba.getElementAtIndex(1);
6786         final double biasZ = ba.getElementAtIndex(2);
6787 
6788         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
6789                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
6790                         true, ba, this);
6791 
6792         // check default values
6793         assertEquals(calibrator.getBiasX(), biasX, 0.0);
6794         assertEquals(calibrator.getBiasY(), biasY, 0.0);
6795         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
6796         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
6797         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6798         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6799         final Acceleration bx2 = new Acceleration(0.0,
6800                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6801         calibrator.getBiasXAsAcceleration(bx2);
6802         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6803         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6804         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
6805         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6806         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6807         final Acceleration by2 = new Acceleration(0.0,
6808                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6809         calibrator.getBiasYAsAcceleration(by2);
6810         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6811         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6812         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
6813         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6814         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6815         final Acceleration bz2 = new Acceleration(0.0,
6816                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6817         calibrator.getBiasZAsAcceleration(bz2);
6818         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6819         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6820         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6821         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6822         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6823         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6824         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6825         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6826         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6827         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6828         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6829         final double[] bias1 = calibrator.getBias();
6830         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6831         final double[] bias2 = new double[3];
6832         calibrator.getBias(bias2);
6833         assertArrayEquals(bias1, bias2, 0.0);
6834         final Matrix b1 = calibrator.getBiasAsMatrix();
6835         assertEquals(b1, ba);
6836         final Matrix b2 = new Matrix(3, 1);
6837         calibrator.getBiasAsMatrix(b2);
6838         assertEquals(b1, b2);
6839         final Matrix ma1 = calibrator.getInitialMa();
6840         assertEquals(ma1, new Matrix(3, 3));
6841         final Matrix ma2 = new Matrix(3, 3);
6842         calibrator.getInitialMa(ma2);
6843         assertEquals(ma1, ma2);
6844         assertSame(calibrator.getMeasurements(), measurements);
6845         assertTrue(calibrator.isCommonAxisUsed());
6846         assertSame(calibrator.getListener(), this);
6847         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
6848         assertFalse(calibrator.isReady());
6849         assertFalse(calibrator.isRunning());
6850         assertNull(calibrator.getEstimatedMa());
6851         assertNull(calibrator.getEstimatedSx());
6852         assertNull(calibrator.getEstimatedSy());
6853         assertNull(calibrator.getEstimatedSz());
6854         assertNull(calibrator.getEstimatedMxy());
6855         assertNull(calibrator.getEstimatedMxz());
6856         assertNull(calibrator.getEstimatedMyx());
6857         assertNull(calibrator.getEstimatedMyz());
6858         assertNull(calibrator.getEstimatedMzx());
6859         assertNull(calibrator.getEstimatedMzy());
6860         assertNull(calibrator.getEstimatedCovariance());
6861         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6862         assertNull(calibrator.getGroundTruthGravityNorm());
6863         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6864         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6865 
6866         // Force IllegalArgumentException
6867         calibrator = null;
6868         try {
6869             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6870                     measurements, true,
6871                     new Matrix(1, 1), this);
6872             fail("IllegalArgumentException expected but not thrown");
6873         } catch (final IllegalArgumentException ignore) {
6874         }
6875         try {
6876             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6877                     measurements, true,
6878                     new Matrix(1, 3), this);
6879             fail("IllegalArgumentException expected but not thrown");
6880         } catch (final IllegalArgumentException ignore) {
6881         }
6882         assertNull(calibrator);
6883     }
6884 
6885     @Test
6886     public void testConstructor71() throws WrongSizeException {
6887         final Matrix ba = generateBa();
6888         final double biasX = ba.getElementAtIndex(0);
6889         final double biasY = ba.getElementAtIndex(1);
6890         final double biasZ = ba.getElementAtIndex(2);
6891 
6892         final Matrix ma = generateMaCommonAxis();
6893         final double sx = ma.getElementAt(0, 0);
6894         final double sy = ma.getElementAt(1, 1);
6895         final double sz = ma.getElementAt(2, 2);
6896         final double mxy = ma.getElementAt(0, 1);
6897         final double mxz = ma.getElementAt(0, 2);
6898         final double myx = ma.getElementAt(1, 0);
6899         final double myz = ma.getElementAt(1, 2);
6900         final double mzx = ma.getElementAt(2, 0);
6901         final double mzy = ma.getElementAt(2, 1);
6902 
6903         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
6904                 new KnownBiasAndGravityNormAccelerometerCalibrator(ba, ma);
6905 
6906         // check default values
6907         assertEquals(calibrator.getBiasX(), biasX, 0.0);
6908         assertEquals(calibrator.getBiasY(), biasY, 0.0);
6909         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
6910         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
6911         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6912         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6913         final Acceleration bx2 = new Acceleration(0.0,
6914                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6915         calibrator.getBiasXAsAcceleration(bx2);
6916         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6917         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6918         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
6919         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6920         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6921         final Acceleration by2 = new Acceleration(0.0,
6922                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6923         calibrator.getBiasYAsAcceleration(by2);
6924         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6925         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6926         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
6927         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6928         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6929         final Acceleration bz2 = new Acceleration(0.0,
6930                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6931         calibrator.getBiasZAsAcceleration(bz2);
6932         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6933         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6934         assertEquals(calibrator.getInitialSx(), sx, 0.0);
6935         assertEquals(calibrator.getInitialSy(), sy, 0.0);
6936         assertEquals(calibrator.getInitialSz(), sz, 0.0);
6937         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
6938         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
6939         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
6940         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
6941         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
6942         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
6943         final double[] bias1 = calibrator.getBias();
6944         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6945         final double[] bias2 = new double[3];
6946         calibrator.getBias(bias2);
6947         assertArrayEquals(bias1, bias2, 0.0);
6948         final Matrix b1 = calibrator.getBiasAsMatrix();
6949         assertEquals(b1, ba);
6950         final Matrix b2 = new Matrix(3, 1);
6951         calibrator.getBiasAsMatrix(b2);
6952         assertEquals(b1, b2);
6953         final Matrix ma1 = new Matrix(3, 3);
6954         ma1.setSubmatrix(0, 0,
6955                 2, 2,
6956                 new double[]{sx, myx, mzx,
6957                         mxy, sy, mzy,
6958                         mxz, myz, sz});
6959         assertEquals(calibrator.getInitialMa(), ma1);
6960         final Matrix ma2 = new Matrix(3, 3);
6961         calibrator.getInitialMa(ma2);
6962         assertEquals(ma1, ma2);
6963         assertNull(calibrator.getMeasurements());
6964         assertFalse(calibrator.isCommonAxisUsed());
6965         assertNull(calibrator.getListener());
6966         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6967         assertFalse(calibrator.isReady());
6968         assertFalse(calibrator.isRunning());
6969         assertNull(calibrator.getEstimatedMa());
6970         assertNull(calibrator.getEstimatedSx());
6971         assertNull(calibrator.getEstimatedSy());
6972         assertNull(calibrator.getEstimatedSz());
6973         assertNull(calibrator.getEstimatedMxy());
6974         assertNull(calibrator.getEstimatedMxz());
6975         assertNull(calibrator.getEstimatedMyx());
6976         assertNull(calibrator.getEstimatedMyz());
6977         assertNull(calibrator.getEstimatedMzx());
6978         assertNull(calibrator.getEstimatedMzy());
6979         assertNull(calibrator.getEstimatedCovariance());
6980         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6981         assertNull(calibrator.getGroundTruthGravityNorm());
6982         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6983         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6984 
6985         // Force IllegalArgumentException
6986         calibrator = null;
6987         try {
6988             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6989                     new Matrix(1, 1), ma);
6990             fail("IllegalArgumentException expected but not thrown");
6991         } catch (final IllegalArgumentException ignore) {
6992         }
6993         try {
6994             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
6995                     new Matrix(1, 3), ma);
6996             fail("IllegalArgumentException expected but not thrown");
6997         } catch (final IllegalArgumentException ignore) {
6998         }
6999         try {
7000             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(ba,
7001                     new Matrix(1, 3));
7002             fail("IllegalArgumentException expected but not thrown");
7003         } catch (final IllegalArgumentException ignore) {
7004         }
7005         try {
7006             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(ba,
7007                     new Matrix(3, 1));
7008             fail("IllegalArgumentException expected but not thrown");
7009         } catch (final IllegalArgumentException ignore) {
7010         }
7011         assertNull(calibrator);
7012     }
7013 
7014     @Test
7015     public void testConstructor72() throws WrongSizeException {
7016         final Matrix ba = generateBa();
7017         final double biasX = ba.getElementAtIndex(0);
7018         final double biasY = ba.getElementAtIndex(1);
7019         final double biasZ = ba.getElementAtIndex(2);
7020 
7021         final Matrix ma = generateMaCommonAxis();
7022         final double sx = ma.getElementAt(0, 0);
7023         final double sy = ma.getElementAt(1, 1);
7024         final double sz = ma.getElementAt(2, 2);
7025         final double mxy = ma.getElementAt(0, 1);
7026         final double mxz = ma.getElementAt(0, 2);
7027         final double myx = ma.getElementAt(1, 0);
7028         final double myz = ma.getElementAt(1, 2);
7029         final double mzx = ma.getElementAt(2, 0);
7030         final double mzy = ma.getElementAt(2, 1);
7031 
7032         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
7033                 new KnownBiasAndGravityNormAccelerometerCalibrator(
7034                         ba, ma, this);
7035 
7036         // check default values
7037         assertEquals(calibrator.getBiasX(), biasX, 0.0);
7038         assertEquals(calibrator.getBiasY(), biasY, 0.0);
7039         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
7040         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
7041         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7042         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7043         final Acceleration bx2 = new Acceleration(0.0,
7044                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7045         calibrator.getBiasXAsAcceleration(bx2);
7046         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7047         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7048         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
7049         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7050         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7051         final Acceleration by2 = new Acceleration(0.0,
7052                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7053         calibrator.getBiasYAsAcceleration(by2);
7054         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7055         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7056         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
7057         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7058         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7059         final Acceleration bz2 = new Acceleration(0.0,
7060                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7061         calibrator.getBiasZAsAcceleration(bz2);
7062         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7063         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7064         assertEquals(calibrator.getInitialSx(), sx, 0.0);
7065         assertEquals(calibrator.getInitialSy(), sy, 0.0);
7066         assertEquals(calibrator.getInitialSz(), sz, 0.0);
7067         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7068         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7069         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7070         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7071         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7072         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7073         final double[] bias1 = calibrator.getBias();
7074         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7075         final double[] bias2 = new double[3];
7076         calibrator.getBias(bias2);
7077         assertArrayEquals(bias1, bias2, 0.0);
7078         final Matrix b1 = calibrator.getBiasAsMatrix();
7079         assertEquals(b1, ba);
7080         final Matrix b2 = new Matrix(3, 1);
7081         calibrator.getBiasAsMatrix(b2);
7082         assertEquals(b1, b2);
7083         final Matrix ma1 = new Matrix(3, 3);
7084         ma1.setSubmatrix(0, 0,
7085                 2, 2,
7086                 new double[]{sx, myx, mzx,
7087                         mxy, sy, mzy,
7088                         mxz, myz, sz});
7089         assertEquals(calibrator.getInitialMa(), ma1);
7090         final Matrix ma2 = new Matrix(3, 3);
7091         calibrator.getInitialMa(ma2);
7092         assertEquals(ma1, ma2);
7093         assertNull(calibrator.getMeasurements());
7094         assertFalse(calibrator.isCommonAxisUsed());
7095         assertSame(calibrator.getListener(), this);
7096         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7097         assertFalse(calibrator.isReady());
7098         assertFalse(calibrator.isRunning());
7099         assertNull(calibrator.getEstimatedMa());
7100         assertNull(calibrator.getEstimatedSx());
7101         assertNull(calibrator.getEstimatedSy());
7102         assertNull(calibrator.getEstimatedSz());
7103         assertNull(calibrator.getEstimatedMxy());
7104         assertNull(calibrator.getEstimatedMxz());
7105         assertNull(calibrator.getEstimatedMyx());
7106         assertNull(calibrator.getEstimatedMyz());
7107         assertNull(calibrator.getEstimatedMzx());
7108         assertNull(calibrator.getEstimatedMzy());
7109         assertNull(calibrator.getEstimatedCovariance());
7110         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7111         assertNull(calibrator.getGroundTruthGravityNorm());
7112         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7113         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7114 
7115         // Force IllegalArgumentException
7116         calibrator = null;
7117         try {
7118             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7119                     new Matrix(1, 1), ma, this);
7120             fail("IllegalArgumentException expected but not thrown");
7121         } catch (final IllegalArgumentException ignore) {
7122         }
7123         try {
7124             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7125                     new Matrix(1, 3), ma, this);
7126             fail("IllegalArgumentException expected but not thrown");
7127         } catch (final IllegalArgumentException ignore) {
7128         }
7129         try {
7130             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(ba,
7131                     new Matrix(1, 3), this);
7132             fail("IllegalArgumentException expected but not thrown");
7133         } catch (final IllegalArgumentException ignore) {
7134         }
7135         try {
7136             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(ba,
7137                     new Matrix(3, 1), this);
7138             fail("IllegalArgumentException expected but not thrown");
7139         } catch (final IllegalArgumentException ignore) {
7140         }
7141         assertNull(calibrator);
7142     }
7143 
7144     @Test
7145     public void testConstructor73() throws WrongSizeException {
7146         final Collection<StandardDeviationBodyKinematics> measurements =
7147                 Collections.emptyList();
7148 
7149         final Matrix ba = generateBa();
7150         final double biasX = ba.getElementAtIndex(0);
7151         final double biasY = ba.getElementAtIndex(1);
7152         final double biasZ = ba.getElementAtIndex(2);
7153 
7154         final Matrix ma = generateMaCommonAxis();
7155         final double sx = ma.getElementAt(0, 0);
7156         final double sy = ma.getElementAt(1, 1);
7157         final double sz = ma.getElementAt(2, 2);
7158         final double mxy = ma.getElementAt(0, 1);
7159         final double mxz = ma.getElementAt(0, 2);
7160         final double myx = ma.getElementAt(1, 0);
7161         final double myz = ma.getElementAt(1, 2);
7162         final double mzx = ma.getElementAt(2, 0);
7163         final double mzy = ma.getElementAt(2, 1);
7164 
7165         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
7166                 new KnownBiasAndGravityNormAccelerometerCalibrator(
7167                         measurements, ba, ma);
7168 
7169         // check default values
7170         assertEquals(calibrator.getBiasX(), biasX, 0.0);
7171         assertEquals(calibrator.getBiasY(), biasY, 0.0);
7172         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
7173         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
7174         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7175         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7176         final Acceleration bx2 = new Acceleration(0.0,
7177                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7178         calibrator.getBiasXAsAcceleration(bx2);
7179         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7180         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7181         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
7182         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7183         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7184         final Acceleration by2 = new Acceleration(0.0,
7185                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7186         calibrator.getBiasYAsAcceleration(by2);
7187         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7188         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7189         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
7190         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7191         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7192         final Acceleration bz2 = new Acceleration(0.0,
7193                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7194         calibrator.getBiasZAsAcceleration(bz2);
7195         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7196         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7197         assertEquals(calibrator.getInitialSx(), sx, 0.0);
7198         assertEquals(calibrator.getInitialSy(), sy, 0.0);
7199         assertEquals(calibrator.getInitialSz(), sz, 0.0);
7200         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7201         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7202         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7203         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7204         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7205         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7206         final double[] bias1 = calibrator.getBias();
7207         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7208         final double[] bias2 = new double[3];
7209         calibrator.getBias(bias2);
7210         assertArrayEquals(bias1, bias2, 0.0);
7211         final Matrix b1 = calibrator.getBiasAsMatrix();
7212         assertEquals(b1, ba);
7213         final Matrix b2 = new Matrix(3, 1);
7214         calibrator.getBiasAsMatrix(b2);
7215         assertEquals(b1, b2);
7216         final Matrix ma1 = new Matrix(3, 3);
7217         ma1.setSubmatrix(0, 0,
7218                 2, 2,
7219                 new double[]{sx, myx, mzx,
7220                         mxy, sy, mzy,
7221                         mxz, myz, sz});
7222         assertEquals(calibrator.getInitialMa(), ma1);
7223         final Matrix ma2 = new Matrix(3, 3);
7224         calibrator.getInitialMa(ma2);
7225         assertEquals(ma1, ma2);
7226         assertSame(calibrator.getMeasurements(), measurements);
7227         assertFalse(calibrator.isCommonAxisUsed());
7228         assertNull(calibrator.getListener());
7229         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7230         assertFalse(calibrator.isReady());
7231         assertFalse(calibrator.isRunning());
7232         assertNull(calibrator.getEstimatedMa());
7233         assertNull(calibrator.getEstimatedSx());
7234         assertNull(calibrator.getEstimatedSy());
7235         assertNull(calibrator.getEstimatedSz());
7236         assertNull(calibrator.getEstimatedMxy());
7237         assertNull(calibrator.getEstimatedMxz());
7238         assertNull(calibrator.getEstimatedMyx());
7239         assertNull(calibrator.getEstimatedMyz());
7240         assertNull(calibrator.getEstimatedMzx());
7241         assertNull(calibrator.getEstimatedMzy());
7242         assertNull(calibrator.getEstimatedCovariance());
7243         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7244         assertNull(calibrator.getGroundTruthGravityNorm());
7245         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7246         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7247 
7248         // Force IllegalArgumentException
7249         calibrator = null;
7250         try {
7251             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7252                     measurements, new Matrix(1, 1), ma);
7253             fail("IllegalArgumentException expected but not thrown");
7254         } catch (final IllegalArgumentException ignore) {
7255         }
7256         try {
7257             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7258                     measurements, new Matrix(1, 3), ma);
7259             fail("IllegalArgumentException expected but not thrown");
7260         } catch (final IllegalArgumentException ignore) {
7261         }
7262         try {
7263             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7264                     measurements, ba, new Matrix(1, 3));
7265             fail("IllegalArgumentException expected but not thrown");
7266         } catch (final IllegalArgumentException ignore) {
7267         }
7268         try {
7269             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7270                     measurements, ba, new Matrix(3, 1));
7271             fail("IllegalArgumentException expected but not thrown");
7272         } catch (final IllegalArgumentException ignore) {
7273         }
7274         assertNull(calibrator);
7275     }
7276 
7277     @Test
7278     public void testConstructor74() throws WrongSizeException {
7279         final Collection<StandardDeviationBodyKinematics> measurements =
7280                 Collections.emptyList();
7281 
7282         final Matrix ba = generateBa();
7283         final double biasX = ba.getElementAtIndex(0);
7284         final double biasY = ba.getElementAtIndex(1);
7285         final double biasZ = ba.getElementAtIndex(2);
7286 
7287         final Matrix ma = generateMaCommonAxis();
7288         final double sx = ma.getElementAt(0, 0);
7289         final double sy = ma.getElementAt(1, 1);
7290         final double sz = ma.getElementAt(2, 2);
7291         final double mxy = ma.getElementAt(0, 1);
7292         final double mxz = ma.getElementAt(0, 2);
7293         final double myx = ma.getElementAt(1, 0);
7294         final double myz = ma.getElementAt(1, 2);
7295         final double mzx = ma.getElementAt(2, 0);
7296         final double mzy = ma.getElementAt(2, 1);
7297 
7298         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
7299                 new KnownBiasAndGravityNormAccelerometerCalibrator(
7300                         measurements, ba, ma, this);
7301 
7302         // check default values
7303         assertEquals(calibrator.getBiasX(), biasX, 0.0);
7304         assertEquals(calibrator.getBiasY(), biasY, 0.0);
7305         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
7306         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
7307         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7308         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7309         final Acceleration bx2 = new Acceleration(0.0,
7310                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7311         calibrator.getBiasXAsAcceleration(bx2);
7312         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7313         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7314         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
7315         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7316         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7317         final Acceleration by2 = new Acceleration(0.0,
7318                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7319         calibrator.getBiasYAsAcceleration(by2);
7320         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7321         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7322         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
7323         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7324         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7325         final Acceleration bz2 = new Acceleration(0.0,
7326                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7327         calibrator.getBiasZAsAcceleration(bz2);
7328         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7329         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7330         assertEquals(calibrator.getInitialSx(), sx, 0.0);
7331         assertEquals(calibrator.getInitialSy(), sy, 0.0);
7332         assertEquals(calibrator.getInitialSz(), sz, 0.0);
7333         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7334         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7335         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7336         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7337         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7338         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7339         final double[] bias1 = calibrator.getBias();
7340         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7341         final double[] bias2 = new double[3];
7342         calibrator.getBias(bias2);
7343         assertArrayEquals(bias1, bias2, 0.0);
7344         final Matrix b1 = calibrator.getBiasAsMatrix();
7345         assertEquals(b1, ba);
7346         final Matrix b2 = new Matrix(3, 1);
7347         calibrator.getBiasAsMatrix(b2);
7348         assertEquals(b1, b2);
7349         final Matrix ma1 = new Matrix(3, 3);
7350         ma1.setSubmatrix(0, 0,
7351                 2, 2,
7352                 new double[]{sx, myx, mzx,
7353                         mxy, sy, mzy,
7354                         mxz, myz, sz});
7355         assertEquals(calibrator.getInitialMa(), ma1);
7356         final Matrix ma2 = new Matrix(3, 3);
7357         calibrator.getInitialMa(ma2);
7358         assertEquals(ma1, ma2);
7359         assertSame(calibrator.getMeasurements(), measurements);
7360         assertFalse(calibrator.isCommonAxisUsed());
7361         assertSame(calibrator.getListener(), this);
7362         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7363         assertFalse(calibrator.isReady());
7364         assertFalse(calibrator.isRunning());
7365         assertNull(calibrator.getEstimatedMa());
7366         assertNull(calibrator.getEstimatedSx());
7367         assertNull(calibrator.getEstimatedSy());
7368         assertNull(calibrator.getEstimatedSz());
7369         assertNull(calibrator.getEstimatedMxy());
7370         assertNull(calibrator.getEstimatedMxz());
7371         assertNull(calibrator.getEstimatedMyx());
7372         assertNull(calibrator.getEstimatedMyz());
7373         assertNull(calibrator.getEstimatedMzx());
7374         assertNull(calibrator.getEstimatedMzy());
7375         assertNull(calibrator.getEstimatedCovariance());
7376         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7377         assertNull(calibrator.getGroundTruthGravityNorm());
7378         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7379         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7380 
7381         // Force IllegalArgumentException
7382         calibrator = null;
7383         try {
7384             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7385                     measurements, new Matrix(1, 1), ma,
7386                     this);
7387             fail("IllegalArgumentException expected but not thrown");
7388         } catch (final IllegalArgumentException ignore) {
7389         }
7390         try {
7391             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7392                     measurements, new Matrix(1, 3), ma,
7393                     this);
7394             fail("IllegalArgumentException expected but not thrown");
7395         } catch (final IllegalArgumentException ignore) {
7396         }
7397         try {
7398             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7399                     measurements, ba, new Matrix(1, 3),
7400                     this);
7401             fail("IllegalArgumentException expected but not thrown");
7402         } catch (final IllegalArgumentException ignore) {
7403         }
7404         try {
7405             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7406                     measurements, ba, new Matrix(3, 1),
7407                     this);
7408             fail("IllegalArgumentException expected but not thrown");
7409         } catch (final IllegalArgumentException ignore) {
7410         }
7411         assertNull(calibrator);
7412     }
7413 
7414     @Test
7415     public void testConstructor75() throws WrongSizeException {
7416         final Matrix ba = generateBa();
7417         final double biasX = ba.getElementAtIndex(0);
7418         final double biasY = ba.getElementAtIndex(1);
7419         final double biasZ = ba.getElementAtIndex(2);
7420 
7421         final Matrix ma = generateMaCommonAxis();
7422         final double sx = ma.getElementAt(0, 0);
7423         final double sy = ma.getElementAt(1, 1);
7424         final double sz = ma.getElementAt(2, 2);
7425         final double mxy = ma.getElementAt(0, 1);
7426         final double mxz = ma.getElementAt(0, 2);
7427         final double myx = ma.getElementAt(1, 0);
7428         final double myz = ma.getElementAt(1, 2);
7429         final double mzx = ma.getElementAt(2, 0);
7430         final double mzy = ma.getElementAt(2, 1);
7431 
7432         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
7433                 new KnownBiasAndGravityNormAccelerometerCalibrator(true, ba, ma);
7434 
7435         // check default values
7436         assertEquals(calibrator.getBiasX(), biasX, 0.0);
7437         assertEquals(calibrator.getBiasY(), biasY, 0.0);
7438         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
7439         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
7440         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7441         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7442         final Acceleration bx2 = new Acceleration(0.0,
7443                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7444         calibrator.getBiasXAsAcceleration(bx2);
7445         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7446         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7447         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
7448         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7449         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7450         final Acceleration by2 = new Acceleration(0.0,
7451                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7452         calibrator.getBiasYAsAcceleration(by2);
7453         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7454         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7455         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
7456         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7457         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7458         final Acceleration bz2 = new Acceleration(0.0,
7459                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7460         calibrator.getBiasZAsAcceleration(bz2);
7461         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7462         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7463         assertEquals(calibrator.getInitialSx(), sx, 0.0);
7464         assertEquals(calibrator.getInitialSy(), sy, 0.0);
7465         assertEquals(calibrator.getInitialSz(), sz, 0.0);
7466         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7467         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7468         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7469         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7470         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7471         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7472         final double[] bias1 = calibrator.getBias();
7473         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7474         final double[] bias2 = new double[3];
7475         calibrator.getBias(bias2);
7476         assertArrayEquals(bias1, bias2, 0.0);
7477         final Matrix b1 = calibrator.getBiasAsMatrix();
7478         assertEquals(b1, ba);
7479         final Matrix b2 = new Matrix(3, 1);
7480         calibrator.getBiasAsMatrix(b2);
7481         assertEquals(b1, b2);
7482         final Matrix ma1 = new Matrix(3, 3);
7483         ma1.setSubmatrix(0, 0,
7484                 2, 2,
7485                 new double[]{sx, myx, mzx,
7486                         mxy, sy, mzy,
7487                         mxz, myz, sz});
7488         assertEquals(calibrator.getInitialMa(), ma1);
7489         final Matrix ma2 = new Matrix(3, 3);
7490         calibrator.getInitialMa(ma2);
7491         assertEquals(ma1, ma2);
7492         assertNull(calibrator.getMeasurements());
7493         assertTrue(calibrator.isCommonAxisUsed());
7494         assertNull(calibrator.getListener());
7495         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
7496         assertFalse(calibrator.isReady());
7497         assertFalse(calibrator.isRunning());
7498         assertNull(calibrator.getEstimatedMa());
7499         assertNull(calibrator.getEstimatedSx());
7500         assertNull(calibrator.getEstimatedSy());
7501         assertNull(calibrator.getEstimatedSz());
7502         assertNull(calibrator.getEstimatedMxy());
7503         assertNull(calibrator.getEstimatedMxz());
7504         assertNull(calibrator.getEstimatedMyx());
7505         assertNull(calibrator.getEstimatedMyz());
7506         assertNull(calibrator.getEstimatedMzx());
7507         assertNull(calibrator.getEstimatedMzy());
7508         assertNull(calibrator.getEstimatedCovariance());
7509         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7510         assertNull(calibrator.getGroundTruthGravityNorm());
7511         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7512         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7513 
7514         // Force IllegalArgumentException
7515         calibrator = null;
7516         try {
7517             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7518                     true, new Matrix(1, 1), ma);
7519             fail("IllegalArgumentException expected but not thrown");
7520         } catch (final IllegalArgumentException ignore) {
7521         }
7522         try {
7523             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7524                     true, new Matrix(1, 3), ma);
7525             fail("IllegalArgumentException expected but not thrown");
7526         } catch (final IllegalArgumentException ignore) {
7527         }
7528         try {
7529             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7530                     true, ba, new Matrix(1, 3));
7531             fail("IllegalArgumentException expected but not thrown");
7532         } catch (final IllegalArgumentException ignore) {
7533         }
7534         try {
7535             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7536                     true, ba, new Matrix(3, 1));
7537             fail("IllegalArgumentException expected but not thrown");
7538         } catch (final IllegalArgumentException ignore) {
7539         }
7540         assertNull(calibrator);
7541     }
7542 
7543     @Test
7544     public void testConstructor76() throws WrongSizeException {
7545         final Matrix ba = generateBa();
7546         final double biasX = ba.getElementAtIndex(0);
7547         final double biasY = ba.getElementAtIndex(1);
7548         final double biasZ = ba.getElementAtIndex(2);
7549 
7550         final Matrix ma = generateMaCommonAxis();
7551         final double sx = ma.getElementAt(0, 0);
7552         final double sy = ma.getElementAt(1, 1);
7553         final double sz = ma.getElementAt(2, 2);
7554         final double mxy = ma.getElementAt(0, 1);
7555         final double mxz = ma.getElementAt(0, 2);
7556         final double myx = ma.getElementAt(1, 0);
7557         final double myz = ma.getElementAt(1, 2);
7558         final double mzx = ma.getElementAt(2, 0);
7559         final double mzy = ma.getElementAt(2, 1);
7560 
7561         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
7562                 new KnownBiasAndGravityNormAccelerometerCalibrator(
7563                         true, ba, ma, this);
7564 
7565         // check default values
7566         assertEquals(calibrator.getBiasX(), biasX, 0.0);
7567         assertEquals(calibrator.getBiasY(), biasY, 0.0);
7568         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
7569         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
7570         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7571         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7572         final Acceleration bx2 = new Acceleration(0.0,
7573                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7574         calibrator.getBiasXAsAcceleration(bx2);
7575         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7576         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7577         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
7578         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7579         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7580         final Acceleration by2 = new Acceleration(0.0,
7581                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7582         calibrator.getBiasYAsAcceleration(by2);
7583         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7584         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7585         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
7586         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7587         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7588         final Acceleration bz2 = new Acceleration(0.0,
7589                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7590         calibrator.getBiasZAsAcceleration(bz2);
7591         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7592         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7593         assertEquals(calibrator.getInitialSx(), sx, 0.0);
7594         assertEquals(calibrator.getInitialSy(), sy, 0.0);
7595         assertEquals(calibrator.getInitialSz(), sz, 0.0);
7596         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7597         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7598         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7599         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7600         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7601         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7602         final double[] bias1 = calibrator.getBias();
7603         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7604         final double[] bias2 = new double[3];
7605         calibrator.getBias(bias2);
7606         assertArrayEquals(bias1, bias2, 0.0);
7607         final Matrix b1 = calibrator.getBiasAsMatrix();
7608         assertEquals(b1, ba);
7609         final Matrix b2 = new Matrix(3, 1);
7610         calibrator.getBiasAsMatrix(b2);
7611         assertEquals(b1, b2);
7612         final Matrix ma1 = new Matrix(3, 3);
7613         ma1.setSubmatrix(0, 0,
7614                 2, 2,
7615                 new double[]{sx, myx, mzx,
7616                         mxy, sy, mzy,
7617                         mxz, myz, sz});
7618         assertEquals(calibrator.getInitialMa(), ma1);
7619         final Matrix ma2 = new Matrix(3, 3);
7620         calibrator.getInitialMa(ma2);
7621         assertEquals(ma1, ma2);
7622         assertNull(calibrator.getMeasurements());
7623         assertTrue(calibrator.isCommonAxisUsed());
7624         assertSame(calibrator.getListener(), this);
7625         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
7626         assertFalse(calibrator.isReady());
7627         assertFalse(calibrator.isRunning());
7628         assertNull(calibrator.getEstimatedMa());
7629         assertNull(calibrator.getEstimatedSx());
7630         assertNull(calibrator.getEstimatedSy());
7631         assertNull(calibrator.getEstimatedSz());
7632         assertNull(calibrator.getEstimatedMxy());
7633         assertNull(calibrator.getEstimatedMxz());
7634         assertNull(calibrator.getEstimatedMyx());
7635         assertNull(calibrator.getEstimatedMyz());
7636         assertNull(calibrator.getEstimatedMzx());
7637         assertNull(calibrator.getEstimatedMzy());
7638         assertNull(calibrator.getEstimatedCovariance());
7639         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7640         assertNull(calibrator.getGroundTruthGravityNorm());
7641         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7642         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7643 
7644         // Force IllegalArgumentException
7645         calibrator = null;
7646         try {
7647             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7648                     true, new Matrix(1, 1), ma,
7649                     this);
7650             fail("IllegalArgumentException expected but not thrown");
7651         } catch (final IllegalArgumentException ignore) {
7652         }
7653         try {
7654             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7655                     true, new Matrix(1, 3), ma,
7656                     this);
7657             fail("IllegalArgumentException expected but not thrown");
7658         } catch (final IllegalArgumentException ignore) {
7659         }
7660         try {
7661             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7662                     true, ba, new Matrix(1, 3),
7663                     this);
7664             fail("IllegalArgumentException expected but not thrown");
7665         } catch (final IllegalArgumentException ignore) {
7666         }
7667         try {
7668             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7669                     true, ba, new Matrix(3, 1),
7670                     this);
7671             fail("IllegalArgumentException expected but not thrown");
7672         } catch (final IllegalArgumentException ignore) {
7673         }
7674         assertNull(calibrator);
7675     }
7676 
7677     @Test
7678     public void testConstructor77() throws WrongSizeException {
7679         final Collection<StandardDeviationBodyKinematics> measurements =
7680                 Collections.emptyList();
7681 
7682         final Matrix ba = generateBa();
7683         final double biasX = ba.getElementAtIndex(0);
7684         final double biasY = ba.getElementAtIndex(1);
7685         final double biasZ = ba.getElementAtIndex(2);
7686 
7687         final Matrix ma = generateMaCommonAxis();
7688         final double sx = ma.getElementAt(0, 0);
7689         final double sy = ma.getElementAt(1, 1);
7690         final double sz = ma.getElementAt(2, 2);
7691         final double mxy = ma.getElementAt(0, 1);
7692         final double mxz = ma.getElementAt(0, 2);
7693         final double myx = ma.getElementAt(1, 0);
7694         final double myz = ma.getElementAt(1, 2);
7695         final double mzx = ma.getElementAt(2, 0);
7696         final double mzy = ma.getElementAt(2, 1);
7697 
7698         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
7699                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
7700                         true, ba, ma);
7701 
7702         // check default values
7703         assertEquals(calibrator.getBiasX(), biasX, 0.0);
7704         assertEquals(calibrator.getBiasY(), biasY, 0.0);
7705         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
7706         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
7707         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7708         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7709         final Acceleration bx2 = new Acceleration(0.0,
7710                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7711         calibrator.getBiasXAsAcceleration(bx2);
7712         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7713         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7714         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
7715         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7716         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7717         final Acceleration by2 = new Acceleration(0.0,
7718                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7719         calibrator.getBiasYAsAcceleration(by2);
7720         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7721         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7722         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
7723         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7724         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7725         final Acceleration bz2 = new Acceleration(0.0,
7726                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7727         calibrator.getBiasZAsAcceleration(bz2);
7728         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7729         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7730         assertEquals(calibrator.getInitialSx(), sx, 0.0);
7731         assertEquals(calibrator.getInitialSy(), sy, 0.0);
7732         assertEquals(calibrator.getInitialSz(), sz, 0.0);
7733         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7734         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7735         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7736         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7737         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7738         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7739         final double[] bias1 = calibrator.getBias();
7740         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7741         final double[] bias2 = new double[3];
7742         calibrator.getBias(bias2);
7743         assertArrayEquals(bias1, bias2, 0.0);
7744         final Matrix b1 = calibrator.getBiasAsMatrix();
7745         assertEquals(b1, ba);
7746         final Matrix b2 = new Matrix(3, 1);
7747         calibrator.getBiasAsMatrix(b2);
7748         assertEquals(b1, b2);
7749         final Matrix ma1 = new Matrix(3, 3);
7750         ma1.setSubmatrix(0, 0,
7751                 2, 2,
7752                 new double[]{sx, myx, mzx,
7753                         mxy, sy, mzy,
7754                         mxz, myz, sz});
7755         assertEquals(calibrator.getInitialMa(), ma1);
7756         final Matrix ma2 = new Matrix(3, 3);
7757         calibrator.getInitialMa(ma2);
7758         assertEquals(ma1, ma2);
7759         assertSame(calibrator.getMeasurements(), measurements);
7760         assertTrue(calibrator.isCommonAxisUsed());
7761         assertNull(calibrator.getListener());
7762         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
7763         assertFalse(calibrator.isReady());
7764         assertFalse(calibrator.isRunning());
7765         assertNull(calibrator.getEstimatedMa());
7766         assertNull(calibrator.getEstimatedSx());
7767         assertNull(calibrator.getEstimatedSy());
7768         assertNull(calibrator.getEstimatedSz());
7769         assertNull(calibrator.getEstimatedMxy());
7770         assertNull(calibrator.getEstimatedMxz());
7771         assertNull(calibrator.getEstimatedMyx());
7772         assertNull(calibrator.getEstimatedMyz());
7773         assertNull(calibrator.getEstimatedMzx());
7774         assertNull(calibrator.getEstimatedMzy());
7775         assertNull(calibrator.getEstimatedCovariance());
7776         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7777         assertNull(calibrator.getGroundTruthGravityNorm());
7778         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7779         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7780 
7781         // Force IllegalArgumentException
7782         calibrator = null;
7783         try {
7784             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7785                     measurements, true,
7786                     new Matrix(1, 1), ma);
7787             fail("IllegalArgumentException expected but not thrown");
7788         } catch (final IllegalArgumentException ignore) {
7789         }
7790         try {
7791             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7792                     measurements, true,
7793                     new Matrix(1, 3), ma);
7794             fail("IllegalArgumentException expected but not thrown");
7795         } catch (final IllegalArgumentException ignore) {
7796         }
7797         try {
7798             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7799                     measurements, true, ba,
7800                     new Matrix(1, 3));
7801             fail("IllegalArgumentException expected but not thrown");
7802         } catch (final IllegalArgumentException ignore) {
7803         }
7804         try {
7805             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7806                     measurements, true, ba,
7807                     new Matrix(3, 1));
7808             fail("IllegalArgumentException expected but not thrown");
7809         } catch (final IllegalArgumentException ignore) {
7810         }
7811         assertNull(calibrator);
7812     }
7813 
7814     @Test
7815     public void testConstructor78() throws WrongSizeException {
7816         final Collection<StandardDeviationBodyKinematics> measurements =
7817                 Collections.emptyList();
7818 
7819         final Matrix ba = generateBa();
7820         final double biasX = ba.getElementAtIndex(0);
7821         final double biasY = ba.getElementAtIndex(1);
7822         final double biasZ = ba.getElementAtIndex(2);
7823 
7824         final Matrix ma = generateMaCommonAxis();
7825         final double sx = ma.getElementAt(0, 0);
7826         final double sy = ma.getElementAt(1, 1);
7827         final double sz = ma.getElementAt(2, 2);
7828         final double mxy = ma.getElementAt(0, 1);
7829         final double mxz = ma.getElementAt(0, 2);
7830         final double myx = ma.getElementAt(1, 0);
7831         final double myz = ma.getElementAt(1, 2);
7832         final double mzx = ma.getElementAt(2, 0);
7833         final double mzy = ma.getElementAt(2, 1);
7834 
7835         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
7836                 new KnownBiasAndGravityNormAccelerometerCalibrator(measurements,
7837                         true, ba, ma, this);
7838 
7839         // check default values
7840         assertEquals(calibrator.getBiasX(), biasX, 0.0);
7841         assertEquals(calibrator.getBiasY(), biasY, 0.0);
7842         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
7843         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
7844         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7845         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7846         final Acceleration bx2 = new Acceleration(0.0,
7847                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7848         calibrator.getBiasXAsAcceleration(bx2);
7849         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7850         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7851         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
7852         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7853         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7854         final Acceleration by2 = new Acceleration(0.0,
7855                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7856         calibrator.getBiasYAsAcceleration(by2);
7857         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7858         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7859         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
7860         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7861         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7862         final Acceleration bz2 = new Acceleration(0.0,
7863                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7864         calibrator.getBiasZAsAcceleration(bz2);
7865         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7866         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7867         assertEquals(calibrator.getInitialSx(), sx, 0.0);
7868         assertEquals(calibrator.getInitialSy(), sy, 0.0);
7869         assertEquals(calibrator.getInitialSz(), sz, 0.0);
7870         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7871         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7872         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7873         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7874         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7875         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7876         final double[] bias1 = calibrator.getBias();
7877         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7878         final double[] bias2 = new double[3];
7879         calibrator.getBias(bias2);
7880         assertArrayEquals(bias1, bias2, 0.0);
7881         final Matrix b1 = calibrator.getBiasAsMatrix();
7882         assertEquals(b1, ba);
7883         final Matrix b2 = new Matrix(3, 1);
7884         calibrator.getBiasAsMatrix(b2);
7885         assertEquals(b1, b2);
7886         final Matrix ma1 = new Matrix(3, 3);
7887         ma1.setSubmatrix(0, 0,
7888                 2, 2,
7889                 new double[]{sx, myx, mzx,
7890                         mxy, sy, mzy,
7891                         mxz, myz, sz});
7892         assertEquals(calibrator.getInitialMa(), ma1);
7893         final Matrix ma2 = new Matrix(3, 3);
7894         calibrator.getInitialMa(ma2);
7895         assertEquals(ma1, ma2);
7896         assertSame(calibrator.getMeasurements(), measurements);
7897         assertTrue(calibrator.isCommonAxisUsed());
7898         assertSame(calibrator.getListener(), this);
7899         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
7900         assertFalse(calibrator.isReady());
7901         assertFalse(calibrator.isRunning());
7902         assertNull(calibrator.getEstimatedMa());
7903         assertNull(calibrator.getEstimatedSx());
7904         assertNull(calibrator.getEstimatedSy());
7905         assertNull(calibrator.getEstimatedSz());
7906         assertNull(calibrator.getEstimatedMxy());
7907         assertNull(calibrator.getEstimatedMxz());
7908         assertNull(calibrator.getEstimatedMyx());
7909         assertNull(calibrator.getEstimatedMyz());
7910         assertNull(calibrator.getEstimatedMzx());
7911         assertNull(calibrator.getEstimatedMzy());
7912         assertNull(calibrator.getEstimatedCovariance());
7913         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7914         assertNull(calibrator.getGroundTruthGravityNorm());
7915         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7916         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7917 
7918         // Force IllegalArgumentException
7919         calibrator = null;
7920         try {
7921             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7922                     measurements, true,
7923                     new Matrix(1, 1), ma,
7924                     this);
7925             fail("IllegalArgumentException expected but not thrown");
7926         } catch (final IllegalArgumentException ignore) {
7927         }
7928         try {
7929             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7930                     measurements, true,
7931                     new Matrix(1, 3), ma,
7932                     this);
7933             fail("IllegalArgumentException expected but not thrown");
7934         } catch (final IllegalArgumentException ignore) {
7935         }
7936         try {
7937             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7938                     measurements, true, ba, new Matrix(1, 3),
7939                     this);
7940             fail("IllegalArgumentException expected but not thrown");
7941         } catch (final IllegalArgumentException ignore) {
7942         }
7943         try {
7944             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
7945                     measurements, true, ba, new Matrix(3, 1),
7946                     this);
7947             fail("IllegalArgumentException expected but not thrown");
7948         } catch (final IllegalArgumentException ignore) {
7949         }
7950         assertNull(calibrator);
7951     }
7952 
7953     @Test
7954     public void testConstructor79() throws WrongSizeException {
7955         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
7956         final double latitude = Math.toRadians(
7957                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
7958         final double longitude = Math.toRadians(
7959                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
7960         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
7961         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
7962         final NEDVelocity nedVelocity = new NEDVelocity();
7963         final ECEFPosition ecefPosition = new ECEFPosition();
7964         final ECEFVelocity ecefVelocity = new ECEFVelocity();
7965         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
7966                 ecefPosition, ecefVelocity);
7967         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
7968                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
7969         final double gravityNorm = gravity.getNorm();
7970 
7971         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
7972                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm);
7973 
7974         // check default values
7975         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
7976         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
7977         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
7978         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
7979         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
7980         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7981         final Acceleration bx2 = new Acceleration(0.0,
7982                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7983         calibrator.getBiasXAsAcceleration(bx2);
7984         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
7985         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7986         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
7987         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
7988         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7989         final Acceleration by2 = new Acceleration(0.0,
7990                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7991         calibrator.getBiasYAsAcceleration(by2);
7992         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
7993         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7994         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
7995         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
7996         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7997         final Acceleration bz2 = new Acceleration(0.0,
7998                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7999         calibrator.getBiasZAsAcceleration(bz2);
8000         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
8001         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8002         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8003         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8004         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8005         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8006         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8007         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8008         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8009         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8010         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8011         final double[] bias1 = calibrator.getBias();
8012         assertArrayEquals(bias1, new double[3], 0.0);
8013         final double[] bias2 = new double[3];
8014         calibrator.getBias(bias2);
8015         assertArrayEquals(bias1, bias2, 0.0);
8016         final Matrix b1 = calibrator.getBiasAsMatrix();
8017         assertEquals(b1, new Matrix(3, 1));
8018         final Matrix b2 = new Matrix(3, 1);
8019         calibrator.getBiasAsMatrix(b2);
8020         assertEquals(b1, b2);
8021         final Matrix ma1 = calibrator.getInitialMa();
8022         assertEquals(ma1, new Matrix(3, 3));
8023         final Matrix ma2 = new Matrix(3, 3);
8024         calibrator.getInitialMa(ma2);
8025         assertEquals(ma1, ma2);
8026         assertNull(calibrator.getMeasurements());
8027         assertFalse(calibrator.isCommonAxisUsed());
8028         assertNull(calibrator.getListener());
8029         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8030         assertFalse(calibrator.isReady());
8031         assertFalse(calibrator.isRunning());
8032         assertNull(calibrator.getEstimatedMa());
8033         assertNull(calibrator.getEstimatedSx());
8034         assertNull(calibrator.getEstimatedSy());
8035         assertNull(calibrator.getEstimatedSz());
8036         assertNull(calibrator.getEstimatedMxy());
8037         assertNull(calibrator.getEstimatedMxz());
8038         assertNull(calibrator.getEstimatedMyx());
8039         assertNull(calibrator.getEstimatedMyz());
8040         assertNull(calibrator.getEstimatedMzx());
8041         assertNull(calibrator.getEstimatedMzy());
8042         assertNull(calibrator.getEstimatedCovariance());
8043         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8044         assertNotNull(calibrator.getGroundTruthGravityNorm());
8045         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8046         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8047         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
8048                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
8049         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
8050         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8051         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8052 
8053         // Force IllegalArgumentException
8054         calibrator = null;
8055         try {
8056             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
8057                     -gravityNorm);
8058             fail("IllegalArgumentException expected but not thrown");
8059         } catch (final IllegalArgumentException ignore) {
8060         }
8061         assertNull(calibrator);
8062     }
8063 
8064     @Test
8065     public void testConstructor80() throws WrongSizeException {
8066         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8067         final double latitude = Math.toRadians(
8068                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8069         final double longitude = Math.toRadians(
8070                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8071         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8072         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8073         final NEDVelocity nedVelocity = new NEDVelocity();
8074         final ECEFPosition ecefPosition = new ECEFPosition();
8075         final ECEFVelocity ecefVelocity = new ECEFVelocity();
8076         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8077                 ecefPosition, ecefVelocity);
8078         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8079                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8080         final double gravityNorm = gravity.getNorm();
8081 
8082         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
8083                 new KnownBiasAndGravityNormAccelerometerCalibrator(
8084                         gravityNorm, this);
8085 
8086         // check default values
8087         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
8088         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
8089         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
8090         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
8091         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
8092         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8093         final Acceleration bx2 = new Acceleration(0.0,
8094                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8095         calibrator.getBiasXAsAcceleration(bx2);
8096         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
8097         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8098         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
8099         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
8100         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8101         final Acceleration by2 = new Acceleration(0.0,
8102                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8103         calibrator.getBiasYAsAcceleration(by2);
8104         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
8105         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8106         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
8107         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
8108         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8109         final Acceleration bz2 = new Acceleration(0.0,
8110                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8111         calibrator.getBiasZAsAcceleration(bz2);
8112         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
8113         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8114         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8115         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8116         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8117         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8118         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8119         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8120         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8121         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8122         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8123         final double[] bias1 = calibrator.getBias();
8124         assertArrayEquals(bias1, new double[3], 0.0);
8125         final double[] bias2 = new double[3];
8126         calibrator.getBias(bias2);
8127         assertArrayEquals(bias1, bias2, 0.0);
8128         final Matrix b1 = calibrator.getBiasAsMatrix();
8129         assertEquals(b1, new Matrix(3, 1));
8130         final Matrix b2 = new Matrix(3, 1);
8131         calibrator.getBiasAsMatrix(b2);
8132         assertEquals(b1, b2);
8133         final Matrix ma1 = calibrator.getInitialMa();
8134         assertEquals(ma1, new Matrix(3, 3));
8135         final Matrix ma2 = new Matrix(3, 3);
8136         calibrator.getInitialMa(ma2);
8137         assertEquals(ma1, ma2);
8138         assertNull(calibrator.getMeasurements());
8139         assertFalse(calibrator.isCommonAxisUsed());
8140         assertSame(calibrator.getListener(), this);
8141         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8142         assertFalse(calibrator.isReady());
8143         assertFalse(calibrator.isRunning());
8144         assertNull(calibrator.getEstimatedMa());
8145         assertNull(calibrator.getEstimatedSx());
8146         assertNull(calibrator.getEstimatedSy());
8147         assertNull(calibrator.getEstimatedSz());
8148         assertNull(calibrator.getEstimatedMxy());
8149         assertNull(calibrator.getEstimatedMxz());
8150         assertNull(calibrator.getEstimatedMyx());
8151         assertNull(calibrator.getEstimatedMyz());
8152         assertNull(calibrator.getEstimatedMzx());
8153         assertNull(calibrator.getEstimatedMzy());
8154         assertNull(calibrator.getEstimatedCovariance());
8155         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8156         assertNotNull(calibrator.getGroundTruthGravityNorm());
8157         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8158         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8159         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
8160                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
8161         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
8162         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8163         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8164 
8165         // Force IllegalArgumentException
8166         calibrator = null;
8167         try {
8168             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
8169                     -gravityNorm, this);
8170             fail("IllegalArgumentException expected but not thrown");
8171         } catch (final IllegalArgumentException ignore) {
8172         }
8173         assertNull(calibrator);
8174     }
8175 
8176     @Test
8177     public void testConstructor81() throws WrongSizeException {
8178         final Collection<StandardDeviationBodyKinematics> measurements =
8179                 Collections.emptyList();
8180 
8181         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8182         final double latitude = Math.toRadians(
8183                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8184         final double longitude = Math.toRadians(
8185                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8186         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8187         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8188         final NEDVelocity nedVelocity = new NEDVelocity();
8189         final ECEFPosition ecefPosition = new ECEFPosition();
8190         final ECEFVelocity ecefVelocity = new ECEFVelocity();
8191         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8192                 ecefPosition, ecefVelocity);
8193         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8194                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8195         final double gravityNorm = gravity.getNorm();
8196 
8197         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
8198                 new KnownBiasAndGravityNormAccelerometerCalibrator(
8199                         gravityNorm, measurements);
8200 
8201         // check default values
8202         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
8203         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
8204         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
8205         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
8206         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
8207         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8208         final Acceleration bx2 = new Acceleration(0.0,
8209                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8210         calibrator.getBiasXAsAcceleration(bx2);
8211         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
8212         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8213         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
8214         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
8215         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8216         final Acceleration by2 = new Acceleration(0.0,
8217                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8218         calibrator.getBiasYAsAcceleration(by2);
8219         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
8220         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8221         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
8222         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
8223         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8224         final Acceleration bz2 = new Acceleration(0.0,
8225                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8226         calibrator.getBiasZAsAcceleration(bz2);
8227         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
8228         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8229         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8230         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8231         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8232         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8233         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8234         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8235         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8236         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8237         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8238         final double[] bias1 = calibrator.getBias();
8239         assertArrayEquals(bias1, new double[3], 0.0);
8240         final double[] bias2 = new double[3];
8241         calibrator.getBias(bias2);
8242         assertArrayEquals(bias1, bias2, 0.0);
8243         final Matrix b1 = calibrator.getBiasAsMatrix();
8244         assertEquals(b1, new Matrix(3, 1));
8245         final Matrix b2 = new Matrix(3, 1);
8246         calibrator.getBiasAsMatrix(b2);
8247         assertEquals(b1, b2);
8248         final Matrix ma1 = calibrator.getInitialMa();
8249         assertEquals(ma1, new Matrix(3, 3));
8250         final Matrix ma2 = new Matrix(3, 3);
8251         calibrator.getInitialMa(ma2);
8252         assertEquals(ma1, ma2);
8253         assertSame(calibrator.getMeasurements(), measurements);
8254         assertFalse(calibrator.isCommonAxisUsed());
8255         assertNull(calibrator.getListener());
8256         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8257         assertFalse(calibrator.isReady());
8258         assertFalse(calibrator.isRunning());
8259         assertNull(calibrator.getEstimatedMa());
8260         assertNull(calibrator.getEstimatedSx());
8261         assertNull(calibrator.getEstimatedSy());
8262         assertNull(calibrator.getEstimatedSz());
8263         assertNull(calibrator.getEstimatedMxy());
8264         assertNull(calibrator.getEstimatedMxz());
8265         assertNull(calibrator.getEstimatedMyx());
8266         assertNull(calibrator.getEstimatedMyz());
8267         assertNull(calibrator.getEstimatedMzx());
8268         assertNull(calibrator.getEstimatedMzy());
8269         assertNull(calibrator.getEstimatedCovariance());
8270         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8271         assertNotNull(calibrator.getGroundTruthGravityNorm());
8272         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8273         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8274         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
8275                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
8276         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
8277         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8278         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8279 
8280         // Force IllegalArgumentException
8281         calibrator = null;
8282         try {
8283             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
8284                     -gravityNorm, measurements);
8285             fail("IllegalArgumentException expected but not thrown");
8286         } catch (final IllegalArgumentException ignore) {
8287         }
8288         assertNull(calibrator);
8289     }
8290 
8291     @Test
8292     public void testConstructor82() throws WrongSizeException {
8293         final Collection<StandardDeviationBodyKinematics> measurements =
8294                 Collections.emptyList();
8295 
8296         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8297         final double latitude = Math.toRadians(
8298                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8299         final double longitude = Math.toRadians(
8300                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8301         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8302         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8303         final NEDVelocity nedVelocity = new NEDVelocity();
8304         final ECEFPosition ecefPosition = new ECEFPosition();
8305         final ECEFVelocity ecefVelocity = new ECEFVelocity();
8306         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8307                 ecefPosition, ecefVelocity);
8308         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8309                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8310         final double gravityNorm = gravity.getNorm();
8311 
8312         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
8313                 new KnownBiasAndGravityNormAccelerometerCalibrator(
8314                         gravityNorm, measurements, this);
8315 
8316         // check default values
8317         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
8318         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
8319         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
8320         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
8321         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
8322         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8323         final Acceleration bx2 = new Acceleration(0.0,
8324                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8325         calibrator.getBiasXAsAcceleration(bx2);
8326         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
8327         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8328         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
8329         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
8330         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8331         final Acceleration by2 = new Acceleration(0.0,
8332                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8333         calibrator.getBiasYAsAcceleration(by2);
8334         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
8335         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8336         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
8337         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
8338         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8339         final Acceleration bz2 = new Acceleration(0.0,
8340                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8341         calibrator.getBiasZAsAcceleration(bz2);
8342         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
8343         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8344         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8345         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8346         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8347         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8348         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8349         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8350         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8351         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8352         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8353         final double[] bias1 = calibrator.getBias();
8354         assertArrayEquals(bias1, new double[3], 0.0);
8355         final double[] bias2 = new double[3];
8356         calibrator.getBias(bias2);
8357         assertArrayEquals(bias1, bias2, 0.0);
8358         final Matrix b1 = calibrator.getBiasAsMatrix();
8359         assertEquals(b1, new Matrix(3, 1));
8360         final Matrix b2 = new Matrix(3, 1);
8361         calibrator.getBiasAsMatrix(b2);
8362         assertEquals(b1, b2);
8363         final Matrix ma1 = calibrator.getInitialMa();
8364         assertEquals(ma1, new Matrix(3, 3));
8365         final Matrix ma2 = new Matrix(3, 3);
8366         calibrator.getInitialMa(ma2);
8367         assertEquals(ma1, ma2);
8368         assertSame(calibrator.getMeasurements(), measurements);
8369         assertFalse(calibrator.isCommonAxisUsed());
8370         assertSame(calibrator.getListener(), this);
8371         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8372         assertFalse(calibrator.isReady());
8373         assertFalse(calibrator.isRunning());
8374         assertNull(calibrator.getEstimatedMa());
8375         assertNull(calibrator.getEstimatedSx());
8376         assertNull(calibrator.getEstimatedSy());
8377         assertNull(calibrator.getEstimatedSz());
8378         assertNull(calibrator.getEstimatedMxy());
8379         assertNull(calibrator.getEstimatedMxz());
8380         assertNull(calibrator.getEstimatedMyx());
8381         assertNull(calibrator.getEstimatedMyz());
8382         assertNull(calibrator.getEstimatedMzx());
8383         assertNull(calibrator.getEstimatedMzy());
8384         assertNull(calibrator.getEstimatedCovariance());
8385         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8386         assertNotNull(calibrator.getGroundTruthGravityNorm());
8387         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8388         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8389         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
8390                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
8391         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
8392         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8393         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8394 
8395         // Force IllegalArgumentException
8396         calibrator = null;
8397         try {
8398             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
8399                     -gravityNorm, measurements, this);
8400             fail("IllegalArgumentException expected but not thrown");
8401         } catch (final IllegalArgumentException ignore) {
8402         }
8403         assertNull(calibrator);
8404     }
8405 
8406     @Test
8407     public void testConstructor83() throws WrongSizeException {
8408         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8409         final double latitude = Math.toRadians(
8410                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8411         final double longitude = Math.toRadians(
8412                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8413         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8414         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8415         final NEDVelocity nedVelocity = new NEDVelocity();
8416         final ECEFPosition ecefPosition = new ECEFPosition();
8417         final ECEFVelocity ecefVelocity = new ECEFVelocity();
8418         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8419                 ecefPosition, ecefVelocity);
8420         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8421                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8422         final double gravityNorm = gravity.getNorm();
8423 
8424         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
8425                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
8426                         true);
8427 
8428         // check default values
8429         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
8430         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
8431         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
8432         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
8433         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
8434         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8435         final Acceleration bx2 = new Acceleration(0.0,
8436                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8437         calibrator.getBiasXAsAcceleration(bx2);
8438         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
8439         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8440         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
8441         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
8442         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8443         final Acceleration by2 = new Acceleration(0.0,
8444                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8445         calibrator.getBiasYAsAcceleration(by2);
8446         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
8447         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8448         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
8449         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
8450         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8451         final Acceleration bz2 = new Acceleration(0.0,
8452                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8453         calibrator.getBiasZAsAcceleration(bz2);
8454         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
8455         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8456         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8457         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8458         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8459         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8460         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8461         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8462         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8463         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8464         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8465         final double[] bias1 = calibrator.getBias();
8466         assertArrayEquals(bias1, new double[3], 0.0);
8467         final double[] bias2 = new double[3];
8468         calibrator.getBias(bias2);
8469         assertArrayEquals(bias1, bias2, 0.0);
8470         final Matrix b1 = calibrator.getBiasAsMatrix();
8471         assertEquals(b1, new Matrix(3, 1));
8472         final Matrix b2 = new Matrix(3, 1);
8473         calibrator.getBiasAsMatrix(b2);
8474         assertEquals(b1, b2);
8475         final Matrix ma1 = calibrator.getInitialMa();
8476         assertEquals(ma1, new Matrix(3, 3));
8477         final Matrix ma2 = new Matrix(3, 3);
8478         calibrator.getInitialMa(ma2);
8479         assertEquals(ma1, ma2);
8480         assertNull(calibrator.getMeasurements());
8481         assertTrue(calibrator.isCommonAxisUsed());
8482         assertNull(calibrator.getListener());
8483         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
8484         assertFalse(calibrator.isReady());
8485         assertFalse(calibrator.isRunning());
8486         assertNull(calibrator.getEstimatedMa());
8487         assertNull(calibrator.getEstimatedSx());
8488         assertNull(calibrator.getEstimatedSy());
8489         assertNull(calibrator.getEstimatedSz());
8490         assertNull(calibrator.getEstimatedMxy());
8491         assertNull(calibrator.getEstimatedMxz());
8492         assertNull(calibrator.getEstimatedMyx());
8493         assertNull(calibrator.getEstimatedMyz());
8494         assertNull(calibrator.getEstimatedMzx());
8495         assertNull(calibrator.getEstimatedMzy());
8496         assertNull(calibrator.getEstimatedCovariance());
8497         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8498         assertNotNull(calibrator.getGroundTruthGravityNorm());
8499         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8500         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8501         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
8502                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
8503         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
8504         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8505         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8506 
8507         // Force IllegalArgumentException
8508         calibrator = null;
8509         try {
8510             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
8511                     -gravityNorm, true);
8512             fail("IllegalArgumentException expected but not thrown");
8513         } catch (final IllegalArgumentException ignore) {
8514         }
8515         assertNull(calibrator);
8516     }
8517 
8518     @Test
8519     public void testConstructor84() throws WrongSizeException {
8520         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8521         final double latitude = Math.toRadians(
8522                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8523         final double longitude = Math.toRadians(
8524                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8525         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8526         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8527         final NEDVelocity nedVelocity = new NEDVelocity();
8528         final ECEFPosition ecefPosition = new ECEFPosition();
8529         final ECEFVelocity ecefVelocity = new ECEFVelocity();
8530         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8531                 ecefPosition, ecefVelocity);
8532         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8533                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8534         final double gravityNorm = gravity.getNorm();
8535 
8536         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
8537                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
8538                         true, this);
8539 
8540         // check default values
8541         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
8542         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
8543         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
8544         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
8545         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
8546         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8547         final Acceleration bx2 = new Acceleration(0.0,
8548                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8549         calibrator.getBiasXAsAcceleration(bx2);
8550         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
8551         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8552         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
8553         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
8554         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8555         final Acceleration by2 = new Acceleration(0.0,
8556                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8557         calibrator.getBiasYAsAcceleration(by2);
8558         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
8559         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8560         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
8561         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
8562         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8563         final Acceleration bz2 = new Acceleration(0.0,
8564                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8565         calibrator.getBiasZAsAcceleration(bz2);
8566         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
8567         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8568         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8569         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8570         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8571         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8572         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8573         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8574         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8575         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8576         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8577         final double[] bias1 = calibrator.getBias();
8578         assertArrayEquals(bias1, new double[3], 0.0);
8579         final double[] bias2 = new double[3];
8580         calibrator.getBias(bias2);
8581         assertArrayEquals(bias1, bias2, 0.0);
8582         final Matrix b1 = calibrator.getBiasAsMatrix();
8583         assertEquals(b1, new Matrix(3, 1));
8584         final Matrix b2 = new Matrix(3, 1);
8585         calibrator.getBiasAsMatrix(b2);
8586         assertEquals(b1, b2);
8587         final Matrix ma1 = calibrator.getInitialMa();
8588         assertEquals(ma1, new Matrix(3, 3));
8589         final Matrix ma2 = new Matrix(3, 3);
8590         calibrator.getInitialMa(ma2);
8591         assertEquals(ma1, ma2);
8592         assertNull(calibrator.getMeasurements());
8593         assertTrue(calibrator.isCommonAxisUsed());
8594         assertSame(calibrator.getListener(), this);
8595         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
8596         assertFalse(calibrator.isReady());
8597         assertFalse(calibrator.isRunning());
8598         assertNull(calibrator.getEstimatedMa());
8599         assertNull(calibrator.getEstimatedSx());
8600         assertNull(calibrator.getEstimatedSy());
8601         assertNull(calibrator.getEstimatedSz());
8602         assertNull(calibrator.getEstimatedMxy());
8603         assertNull(calibrator.getEstimatedMxz());
8604         assertNull(calibrator.getEstimatedMyx());
8605         assertNull(calibrator.getEstimatedMyz());
8606         assertNull(calibrator.getEstimatedMzx());
8607         assertNull(calibrator.getEstimatedMzy());
8608         assertNull(calibrator.getEstimatedCovariance());
8609         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8610         assertNotNull(calibrator.getGroundTruthGravityNorm());
8611         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8612         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8613         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
8614                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
8615         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
8616         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8617         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8618 
8619         // Force IllegalArgumentException
8620         calibrator = null;
8621         try {
8622             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
8623                     -gravityNorm, true, this);
8624             fail("IllegalArgumentException expected but not thrown");
8625         } catch (final IllegalArgumentException ignore) {
8626         }
8627         assertNull(calibrator);
8628     }
8629 
8630     @Test
8631     public void testConstructor85() throws WrongSizeException {
8632         final Collection<StandardDeviationBodyKinematics> measurements =
8633                 Collections.emptyList();
8634         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8635         final double latitude = Math.toRadians(
8636                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8637         final double longitude = Math.toRadians(
8638                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8639         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8640         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8641         final NEDVelocity nedVelocity = new NEDVelocity();
8642         final ECEFPosition ecefPosition = new ECEFPosition();
8643         final ECEFVelocity ecefVelocity = new ECEFVelocity();
8644         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8645                 ecefPosition, ecefVelocity);
8646         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8647                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8648         final double gravityNorm = gravity.getNorm();
8649 
8650         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
8651                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
8652                         measurements, true);
8653 
8654         // check default values
8655         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
8656         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
8657         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
8658         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
8659         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
8660         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8661         final Acceleration bx2 = new Acceleration(0.0,
8662                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8663         calibrator.getBiasXAsAcceleration(bx2);
8664         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
8665         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8666         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
8667         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
8668         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8669         final Acceleration by2 = new Acceleration(0.0,
8670                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8671         calibrator.getBiasYAsAcceleration(by2);
8672         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
8673         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8674         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
8675         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
8676         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8677         final Acceleration bz2 = new Acceleration(0.0,
8678                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8679         calibrator.getBiasZAsAcceleration(bz2);
8680         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
8681         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8682         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8683         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8684         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8685         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8686         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8687         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8688         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8689         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8690         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8691         final double[] bias1 = calibrator.getBias();
8692         assertArrayEquals(bias1, new double[3], 0.0);
8693         final double[] bias2 = new double[3];
8694         calibrator.getBias(bias2);
8695         assertArrayEquals(bias1, bias2, 0.0);
8696         final Matrix b1 = calibrator.getBiasAsMatrix();
8697         assertEquals(b1, new Matrix(3, 1));
8698         final Matrix b2 = new Matrix(3, 1);
8699         calibrator.getBiasAsMatrix(b2);
8700         assertEquals(b1, b2);
8701         final Matrix ma1 = calibrator.getInitialMa();
8702         assertEquals(ma1, new Matrix(3, 3));
8703         final Matrix ma2 = new Matrix(3, 3);
8704         calibrator.getInitialMa(ma2);
8705         assertEquals(ma1, ma2);
8706         assertSame(calibrator.getMeasurements(), measurements);
8707         assertTrue(calibrator.isCommonAxisUsed());
8708         assertNull(calibrator.getListener());
8709         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
8710         assertFalse(calibrator.isReady());
8711         assertFalse(calibrator.isRunning());
8712         assertNull(calibrator.getEstimatedMa());
8713         assertNull(calibrator.getEstimatedSx());
8714         assertNull(calibrator.getEstimatedSy());
8715         assertNull(calibrator.getEstimatedSz());
8716         assertNull(calibrator.getEstimatedMxy());
8717         assertNull(calibrator.getEstimatedMxz());
8718         assertNull(calibrator.getEstimatedMyx());
8719         assertNull(calibrator.getEstimatedMyz());
8720         assertNull(calibrator.getEstimatedMzx());
8721         assertNull(calibrator.getEstimatedMzy());
8722         assertNull(calibrator.getEstimatedCovariance());
8723         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8724         assertNotNull(calibrator.getGroundTruthGravityNorm());
8725         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8726         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8727         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
8728                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
8729         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
8730         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8731         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8732 
8733         // Force IllegalArgumentException
8734         calibrator = null;
8735         try {
8736             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
8737                     -gravityNorm, measurements, true);
8738             fail("IllegalArgumentException expected but not thrown");
8739         } catch (final IllegalArgumentException ignore) {
8740         }
8741         assertNull(calibrator);
8742     }
8743 
8744     @Test
8745     public void testConstructor86() throws WrongSizeException {
8746         final Collection<StandardDeviationBodyKinematics> measurements =
8747                 Collections.emptyList();
8748         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8749         final double latitude = Math.toRadians(
8750                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8751         final double longitude = Math.toRadians(
8752                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8753         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8754         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8755         final NEDVelocity nedVelocity = new NEDVelocity();
8756         final ECEFPosition ecefPosition = new ECEFPosition();
8757         final ECEFVelocity ecefVelocity = new ECEFVelocity();
8758         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8759                 ecefPosition, ecefVelocity);
8760         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8761                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8762         final double gravityNorm = gravity.getNorm();
8763 
8764         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
8765                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
8766                         measurements, true, this);
8767 
8768         // check default values
8769         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
8770         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
8771         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
8772         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
8773         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
8774         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8775         final Acceleration bx2 = new Acceleration(0.0,
8776                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8777         calibrator.getBiasXAsAcceleration(bx2);
8778         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
8779         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8780         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
8781         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
8782         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8783         final Acceleration by2 = new Acceleration(0.0,
8784                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8785         calibrator.getBiasYAsAcceleration(by2);
8786         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
8787         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8788         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
8789         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
8790         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8791         final Acceleration bz2 = new Acceleration(0.0,
8792                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8793         calibrator.getBiasZAsAcceleration(bz2);
8794         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
8795         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8796         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8797         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8798         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8799         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8800         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8801         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8802         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8803         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8804         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8805         final double[] bias1 = calibrator.getBias();
8806         assertArrayEquals(bias1, new double[3], 0.0);
8807         final double[] bias2 = new double[3];
8808         calibrator.getBias(bias2);
8809         assertArrayEquals(bias1, bias2, 0.0);
8810         final Matrix b1 = calibrator.getBiasAsMatrix();
8811         assertEquals(b1, new Matrix(3, 1));
8812         final Matrix b2 = new Matrix(3, 1);
8813         calibrator.getBiasAsMatrix(b2);
8814         assertEquals(b1, b2);
8815         final Matrix ma1 = calibrator.getInitialMa();
8816         assertEquals(ma1, new Matrix(3, 3));
8817         final Matrix ma2 = new Matrix(3, 3);
8818         calibrator.getInitialMa(ma2);
8819         assertEquals(ma1, ma2);
8820         assertSame(calibrator.getMeasurements(), measurements);
8821         assertTrue(calibrator.isCommonAxisUsed());
8822         assertSame(calibrator.getListener(), this);
8823         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
8824         assertFalse(calibrator.isReady());
8825         assertFalse(calibrator.isRunning());
8826         assertNull(calibrator.getEstimatedMa());
8827         assertNull(calibrator.getEstimatedSx());
8828         assertNull(calibrator.getEstimatedSy());
8829         assertNull(calibrator.getEstimatedSz());
8830         assertNull(calibrator.getEstimatedMxy());
8831         assertNull(calibrator.getEstimatedMxz());
8832         assertNull(calibrator.getEstimatedMyx());
8833         assertNull(calibrator.getEstimatedMyz());
8834         assertNull(calibrator.getEstimatedMzx());
8835         assertNull(calibrator.getEstimatedMzy());
8836         assertNull(calibrator.getEstimatedCovariance());
8837         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8838         assertNotNull(calibrator.getGroundTruthGravityNorm());
8839         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8840         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8841         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
8842                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
8843         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
8844         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8845         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8846 
8847         // Force IllegalArgumentException
8848         calibrator = null;
8849         try {
8850             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
8851                     -gravityNorm, measurements, true, this);
8852             fail("IllegalArgumentException expected but not thrown");
8853         } catch (final IllegalArgumentException ignore) {
8854         }
8855         assertNull(calibrator);
8856     }
8857 
8858     @Test
8859     public void testConstructor87() throws WrongSizeException {
8860         final Matrix ba = generateBa();
8861         final double biasX = ba.getElementAtIndex(0);
8862         final double biasY = ba.getElementAtIndex(1);
8863         final double biasZ = ba.getElementAtIndex(2);
8864 
8865         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8866         final double latitude = Math.toRadians(
8867                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8868         final double longitude = Math.toRadians(
8869                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8870         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8871         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8872         final NEDVelocity nedVelocity = new NEDVelocity();
8873         final ECEFPosition ecefPosition = new ECEFPosition();
8874         final ECEFVelocity ecefVelocity = new ECEFVelocity();
8875         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8876                 ecefPosition, ecefVelocity);
8877         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8878                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8879         final double gravityNorm = gravity.getNorm();
8880 
8881         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
8882                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
8883                         biasX, biasY, biasZ);
8884 
8885         // check default values
8886         assertEquals(calibrator.getBiasX(), biasX, 0.0);
8887         assertEquals(calibrator.getBiasY(), biasY, 0.0);
8888         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
8889         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
8890         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8891         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8892         final Acceleration bx2 = new Acceleration(0.0,
8893                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8894         calibrator.getBiasXAsAcceleration(bx2);
8895         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8896         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8897         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
8898         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8899         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8900         final Acceleration by2 = new Acceleration(0.0,
8901                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8902         calibrator.getBiasYAsAcceleration(by2);
8903         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8904         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8905         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
8906         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8907         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8908         final Acceleration bz2 = new Acceleration(0.0,
8909                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8910         calibrator.getBiasZAsAcceleration(bz2);
8911         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8912         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8913         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8914         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8915         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8916         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8917         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8918         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8919         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8920         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8921         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8922         final double[] bias1 = calibrator.getBias();
8923         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8924         final double[] bias2 = new double[3];
8925         calibrator.getBias(bias2);
8926         assertArrayEquals(bias1, bias2, 0.0);
8927         final Matrix b1 = calibrator.getBiasAsMatrix();
8928         assertEquals(b1, ba);
8929         final Matrix b2 = new Matrix(3, 1);
8930         calibrator.getBiasAsMatrix(b2);
8931         assertEquals(b1, b2);
8932         final Matrix ma1 = calibrator.getInitialMa();
8933         assertEquals(ma1, new Matrix(3, 3));
8934         final Matrix ma2 = new Matrix(3, 3);
8935         calibrator.getInitialMa(ma2);
8936         assertEquals(ma1, ma2);
8937         assertNull(calibrator.getMeasurements());
8938         assertFalse(calibrator.isCommonAxisUsed());
8939         assertNull(calibrator.getListener());
8940         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8941         assertFalse(calibrator.isReady());
8942         assertFalse(calibrator.isRunning());
8943         assertNull(calibrator.getEstimatedMa());
8944         assertNull(calibrator.getEstimatedSx());
8945         assertNull(calibrator.getEstimatedSy());
8946         assertNull(calibrator.getEstimatedSz());
8947         assertNull(calibrator.getEstimatedMxy());
8948         assertNull(calibrator.getEstimatedMxz());
8949         assertNull(calibrator.getEstimatedMyx());
8950         assertNull(calibrator.getEstimatedMyz());
8951         assertNull(calibrator.getEstimatedMzx());
8952         assertNull(calibrator.getEstimatedMzy());
8953         assertNull(calibrator.getEstimatedCovariance());
8954         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8955         assertNotNull(calibrator.getGroundTruthGravityNorm());
8956         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8957         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8958         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
8959                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
8960         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
8961         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8962         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8963 
8964         // Force IllegalArgumentException
8965         calibrator = null;
8966         try {
8967             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
8968                     -gravityNorm, biasX, biasY, biasZ);
8969             fail("IllegalArgumentException expected but not thrown");
8970         } catch (final IllegalArgumentException ignore) {
8971         }
8972         assertNull(calibrator);
8973     }
8974 
8975     @Test
8976     public void testConstructor88() throws WrongSizeException {
8977         final Matrix ba = generateBa();
8978         final double biasX = ba.getElementAtIndex(0);
8979         final double biasY = ba.getElementAtIndex(1);
8980         final double biasZ = ba.getElementAtIndex(2);
8981 
8982         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8983         final double latitude = Math.toRadians(
8984                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8985         final double longitude = Math.toRadians(
8986                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8987         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8988         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8989         final NEDVelocity nedVelocity = new NEDVelocity();
8990         final ECEFPosition ecefPosition = new ECEFPosition();
8991         final ECEFVelocity ecefVelocity = new ECEFVelocity();
8992         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8993                 ecefPosition, ecefVelocity);
8994         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8995                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8996         final double gravityNorm = gravity.getNorm();
8997 
8998         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
8999                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
9000                         biasX, biasY, biasZ, this);
9001 
9002         // check default values
9003         assertEquals(calibrator.getBiasX(), biasX, 0.0);
9004         assertEquals(calibrator.getBiasY(), biasY, 0.0);
9005         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
9006         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
9007         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9008         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9009         final Acceleration bx2 = new Acceleration(0.0,
9010                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9011         calibrator.getBiasXAsAcceleration(bx2);
9012         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9013         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9014         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
9015         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9016         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9017         final Acceleration by2 = new Acceleration(0.0,
9018                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9019         calibrator.getBiasYAsAcceleration(by2);
9020         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9021         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9022         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
9023         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9024         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9025         final Acceleration bz2 = new Acceleration(0.0,
9026                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9027         calibrator.getBiasZAsAcceleration(bz2);
9028         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9029         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9030         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9031         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9032         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9033         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9034         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9035         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9036         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9037         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9038         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9039         final double[] bias1 = calibrator.getBias();
9040         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9041         final double[] bias2 = new double[3];
9042         calibrator.getBias(bias2);
9043         assertArrayEquals(bias1, bias2, 0.0);
9044         final Matrix b1 = calibrator.getBiasAsMatrix();
9045         assertEquals(b1, ba);
9046         final Matrix b2 = new Matrix(3, 1);
9047         calibrator.getBiasAsMatrix(b2);
9048         assertEquals(b1, b2);
9049         final Matrix ma1 = calibrator.getInitialMa();
9050         assertEquals(ma1, new Matrix(3, 3));
9051         final Matrix ma2 = new Matrix(3, 3);
9052         calibrator.getInitialMa(ma2);
9053         assertEquals(ma1, ma2);
9054         assertNull(calibrator.getMeasurements());
9055         assertFalse(calibrator.isCommonAxisUsed());
9056         assertSame(calibrator.getListener(), this);
9057         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9058         assertFalse(calibrator.isReady());
9059         assertFalse(calibrator.isRunning());
9060         assertNull(calibrator.getEstimatedMa());
9061         assertNull(calibrator.getEstimatedSx());
9062         assertNull(calibrator.getEstimatedSy());
9063         assertNull(calibrator.getEstimatedSz());
9064         assertNull(calibrator.getEstimatedMxy());
9065         assertNull(calibrator.getEstimatedMxz());
9066         assertNull(calibrator.getEstimatedMyx());
9067         assertNull(calibrator.getEstimatedMyz());
9068         assertNull(calibrator.getEstimatedMzx());
9069         assertNull(calibrator.getEstimatedMzy());
9070         assertNull(calibrator.getEstimatedCovariance());
9071         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9072         assertNotNull(calibrator.getGroundTruthGravityNorm());
9073         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9074         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9075         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9076                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9077         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9078         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9079         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9080 
9081         // Force IllegalArgumentException
9082         calibrator = null;
9083         try {
9084             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
9085                     -gravityNorm, biasX, biasY, biasZ, this);
9086             fail("IllegalArgumentException expected but not thrown");
9087         } catch (final IllegalArgumentException ignore) {
9088         }
9089         assertNull(calibrator);
9090     }
9091 
9092     @Test
9093     public void testConstructor89() throws WrongSizeException {
9094         final Collection<StandardDeviationBodyKinematics> measurements =
9095                 Collections.emptyList();
9096 
9097         final Matrix ba = generateBa();
9098         final double biasX = ba.getElementAtIndex(0);
9099         final double biasY = ba.getElementAtIndex(1);
9100         final double biasZ = ba.getElementAtIndex(2);
9101 
9102         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9103         final double latitude = Math.toRadians(
9104                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9105         final double longitude = Math.toRadians(
9106                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9107         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9108         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9109         final NEDVelocity nedVelocity = new NEDVelocity();
9110         final ECEFPosition ecefPosition = new ECEFPosition();
9111         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9112         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9113                 ecefPosition, ecefVelocity);
9114         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9115                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9116         final double gravityNorm = gravity.getNorm();
9117 
9118         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
9119                 new KnownBiasAndGravityNormAccelerometerCalibrator(
9120                         gravityNorm, measurements,
9121                         biasX, biasY, biasZ);
9122 
9123         // check default values
9124         assertEquals(calibrator.getBiasX(), biasX, 0.0);
9125         assertEquals(calibrator.getBiasY(), biasY, 0.0);
9126         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
9127         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
9128         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9129         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9130         final Acceleration bx2 = new Acceleration(0.0,
9131                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9132         calibrator.getBiasXAsAcceleration(bx2);
9133         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9134         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9135         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
9136         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9137         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9138         final Acceleration by2 = new Acceleration(0.0,
9139                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9140         calibrator.getBiasYAsAcceleration(by2);
9141         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9142         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9143         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
9144         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9145         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9146         final Acceleration bz2 = new Acceleration(0.0,
9147                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9148         calibrator.getBiasZAsAcceleration(bz2);
9149         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9150         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9151         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9152         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9153         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9154         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9155         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9156         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9157         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9158         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9159         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9160         final double[] bias1 = calibrator.getBias();
9161         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9162         final double[] bias2 = new double[3];
9163         calibrator.getBias(bias2);
9164         assertArrayEquals(bias1, bias2, 0.0);
9165         final Matrix b1 = calibrator.getBiasAsMatrix();
9166         assertEquals(b1, ba);
9167         final Matrix b2 = new Matrix(3, 1);
9168         calibrator.getBiasAsMatrix(b2);
9169         assertEquals(b1, b2);
9170         final Matrix ma1 = calibrator.getInitialMa();
9171         assertEquals(ma1, new Matrix(3, 3));
9172         final Matrix ma2 = new Matrix(3, 3);
9173         calibrator.getInitialMa(ma2);
9174         assertEquals(ma1, ma2);
9175         assertSame(calibrator.getMeasurements(), measurements);
9176         assertFalse(calibrator.isCommonAxisUsed());
9177         assertNull(calibrator.getListener());
9178         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9179         assertFalse(calibrator.isReady());
9180         assertFalse(calibrator.isRunning());
9181         assertNull(calibrator.getEstimatedMa());
9182         assertNull(calibrator.getEstimatedSx());
9183         assertNull(calibrator.getEstimatedSy());
9184         assertNull(calibrator.getEstimatedSz());
9185         assertNull(calibrator.getEstimatedMxy());
9186         assertNull(calibrator.getEstimatedMxz());
9187         assertNull(calibrator.getEstimatedMyx());
9188         assertNull(calibrator.getEstimatedMyz());
9189         assertNull(calibrator.getEstimatedMzx());
9190         assertNull(calibrator.getEstimatedMzy());
9191         assertNull(calibrator.getEstimatedCovariance());
9192         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9193         assertNotNull(calibrator.getGroundTruthGravityNorm());
9194         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9195         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9196         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9197                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9198         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9199         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9200         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9201 
9202         // Force IllegalArgumentException
9203         calibrator = null;
9204         try {
9205             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
9206                     -gravityNorm, measurements,
9207                     biasX, biasY, biasZ);
9208             fail("IllegalArgumentException expected but not thrown");
9209         } catch (final IllegalArgumentException ignore) {
9210         }
9211         assertNull(calibrator);
9212     }
9213 
9214     @Test
9215     public void testConstructor90() throws WrongSizeException {
9216         final Collection<StandardDeviationBodyKinematics> measurements =
9217                 Collections.emptyList();
9218 
9219         final Matrix ba = generateBa();
9220         final double biasX = ba.getElementAtIndex(0);
9221         final double biasY = ba.getElementAtIndex(1);
9222         final double biasZ = ba.getElementAtIndex(2);
9223 
9224         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9225         final double latitude = Math.toRadians(
9226                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9227         final double longitude = Math.toRadians(
9228                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9229         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9230         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9231         final NEDVelocity nedVelocity = new NEDVelocity();
9232         final ECEFPosition ecefPosition = new ECEFPosition();
9233         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9234         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9235                 ecefPosition, ecefVelocity);
9236         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9237                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9238         final double gravityNorm = gravity.getNorm();
9239 
9240         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
9241                 new KnownBiasAndGravityNormAccelerometerCalibrator(
9242                         gravityNorm, measurements,
9243                         biasX, biasY, biasZ, this);
9244 
9245         // check default values
9246         assertEquals(calibrator.getBiasX(), biasX, 0.0);
9247         assertEquals(calibrator.getBiasY(), biasY, 0.0);
9248         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
9249         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
9250         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9251         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9252         final Acceleration bx2 = new Acceleration(0.0,
9253                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9254         calibrator.getBiasXAsAcceleration(bx2);
9255         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9256         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9257         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
9258         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9259         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9260         final Acceleration by2 = new Acceleration(0.0,
9261                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9262         calibrator.getBiasYAsAcceleration(by2);
9263         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9264         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9265         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
9266         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9267         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9268         final Acceleration bz2 = new Acceleration(0.0,
9269                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9270         calibrator.getBiasZAsAcceleration(bz2);
9271         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9272         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9273         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9274         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9275         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9276         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9277         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9278         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9279         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9280         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9281         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9282         final double[] bias1 = calibrator.getBias();
9283         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9284         final double[] bias2 = new double[3];
9285         calibrator.getBias(bias2);
9286         assertArrayEquals(bias1, bias2, 0.0);
9287         final Matrix b1 = calibrator.getBiasAsMatrix();
9288         assertEquals(b1, ba);
9289         final Matrix b2 = new Matrix(3, 1);
9290         calibrator.getBiasAsMatrix(b2);
9291         assertEquals(b1, b2);
9292         final Matrix ma1 = calibrator.getInitialMa();
9293         assertEquals(ma1, new Matrix(3, 3));
9294         final Matrix ma2 = new Matrix(3, 3);
9295         calibrator.getInitialMa(ma2);
9296         assertEquals(ma1, ma2);
9297         assertSame(calibrator.getMeasurements(), measurements);
9298         assertFalse(calibrator.isCommonAxisUsed());
9299         assertSame(calibrator.getListener(), this);
9300         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9301         assertFalse(calibrator.isReady());
9302         assertFalse(calibrator.isRunning());
9303         assertNull(calibrator.getEstimatedMa());
9304         assertNull(calibrator.getEstimatedSx());
9305         assertNull(calibrator.getEstimatedSy());
9306         assertNull(calibrator.getEstimatedSz());
9307         assertNull(calibrator.getEstimatedMxy());
9308         assertNull(calibrator.getEstimatedMxz());
9309         assertNull(calibrator.getEstimatedMyx());
9310         assertNull(calibrator.getEstimatedMyz());
9311         assertNull(calibrator.getEstimatedMzx());
9312         assertNull(calibrator.getEstimatedMzy());
9313         assertNull(calibrator.getEstimatedCovariance());
9314         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9315         assertNotNull(calibrator.getGroundTruthGravityNorm());
9316         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9317         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9318         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9319                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9320         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9321         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9322         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9323 
9324         // Force IllegalArgumentException
9325         calibrator = null;
9326         try {
9327             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
9328                     -gravityNorm, measurements,
9329                     biasX, biasY, biasZ, this);
9330             fail("IllegalArgumentException expected but not thrown");
9331         } catch (final IllegalArgumentException ignore) {
9332         }
9333         assertNull(calibrator);
9334     }
9335 
9336     @Test
9337     public void testConstructor91() throws WrongSizeException {
9338         final Matrix ba = generateBa();
9339         final double biasX = ba.getElementAtIndex(0);
9340         final double biasY = ba.getElementAtIndex(1);
9341         final double biasZ = ba.getElementAtIndex(2);
9342 
9343         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9344         final double latitude = Math.toRadians(
9345                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9346         final double longitude = Math.toRadians(
9347                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9348         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9349         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9350         final NEDVelocity nedVelocity = new NEDVelocity();
9351         final ECEFPosition ecefPosition = new ECEFPosition();
9352         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9353         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9354                 ecefPosition, ecefVelocity);
9355         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9356                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9357         final double gravityNorm = gravity.getNorm();
9358 
9359         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
9360                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
9361                         true, biasX, biasY, biasZ);
9362 
9363         // check default values
9364         assertEquals(calibrator.getBiasX(), biasX, 0.0);
9365         assertEquals(calibrator.getBiasY(), biasY, 0.0);
9366         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
9367         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
9368         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9369         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9370         final Acceleration bx2 = new Acceleration(0.0,
9371                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9372         calibrator.getBiasXAsAcceleration(bx2);
9373         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9374         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9375         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
9376         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9377         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9378         final Acceleration by2 = new Acceleration(0.0,
9379                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9380         calibrator.getBiasYAsAcceleration(by2);
9381         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9382         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9383         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
9384         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9385         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9386         final Acceleration bz2 = new Acceleration(0.0,
9387                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9388         calibrator.getBiasZAsAcceleration(bz2);
9389         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9390         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9391         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9392         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9393         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9394         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9395         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9396         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9397         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9398         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9399         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9400         final double[] bias1 = calibrator.getBias();
9401         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9402         final double[] bias2 = new double[3];
9403         calibrator.getBias(bias2);
9404         assertArrayEquals(bias1, bias2, 0.0);
9405         final Matrix b1 = calibrator.getBiasAsMatrix();
9406         assertEquals(b1, ba);
9407         final Matrix b2 = new Matrix(3, 1);
9408         calibrator.getBiasAsMatrix(b2);
9409         assertEquals(b1, b2);
9410         final Matrix ma1 = calibrator.getInitialMa();
9411         assertEquals(ma1, new Matrix(3, 3));
9412         final Matrix ma2 = new Matrix(3, 3);
9413         calibrator.getInitialMa(ma2);
9414         assertEquals(ma1, ma2);
9415         assertNull(calibrator.getMeasurements());
9416         assertTrue(calibrator.isCommonAxisUsed());
9417         assertNull(calibrator.getListener());
9418         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
9419         assertFalse(calibrator.isReady());
9420         assertFalse(calibrator.isRunning());
9421         assertNull(calibrator.getEstimatedMa());
9422         assertNull(calibrator.getEstimatedSx());
9423         assertNull(calibrator.getEstimatedSy());
9424         assertNull(calibrator.getEstimatedSz());
9425         assertNull(calibrator.getEstimatedMxy());
9426         assertNull(calibrator.getEstimatedMxz());
9427         assertNull(calibrator.getEstimatedMyx());
9428         assertNull(calibrator.getEstimatedMyz());
9429         assertNull(calibrator.getEstimatedMzx());
9430         assertNull(calibrator.getEstimatedMzy());
9431         assertNull(calibrator.getEstimatedCovariance());
9432         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9433         assertNotNull(calibrator.getGroundTruthGravityNorm());
9434         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9435         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9436         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9437                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9438         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9439         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9440         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9441 
9442         // Force IllegalArgumentException
9443         calibrator = null;
9444         try {
9445             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
9446                     -gravityNorm, true, biasX, biasY, biasZ);
9447             fail("IllegalArgumentException expected but not thrown");
9448         } catch (final IllegalArgumentException ignore) {
9449         }
9450         assertNull(calibrator);
9451     }
9452 
9453     @Test
9454     public void testConstructor92() throws WrongSizeException {
9455         final Matrix ba = generateBa();
9456         final double biasX = ba.getElementAtIndex(0);
9457         final double biasY = ba.getElementAtIndex(1);
9458         final double biasZ = ba.getElementAtIndex(2);
9459 
9460         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9461         final double latitude = Math.toRadians(
9462                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9463         final double longitude = Math.toRadians(
9464                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9465         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9466         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9467         final NEDVelocity nedVelocity = new NEDVelocity();
9468         final ECEFPosition ecefPosition = new ECEFPosition();
9469         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9470         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9471                 ecefPosition, ecefVelocity);
9472         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9473                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9474         final double gravityNorm = gravity.getNorm();
9475 
9476         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
9477                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
9478                         true, biasX, biasY, biasZ,
9479                         this);
9480 
9481         // check default values
9482         assertEquals(calibrator.getBiasX(), biasX, 0.0);
9483         assertEquals(calibrator.getBiasY(), biasY, 0.0);
9484         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
9485         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
9486         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9487         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9488         final Acceleration bx2 = new Acceleration(0.0,
9489                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9490         calibrator.getBiasXAsAcceleration(bx2);
9491         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9492         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9493         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
9494         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9495         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9496         final Acceleration by2 = new Acceleration(0.0,
9497                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9498         calibrator.getBiasYAsAcceleration(by2);
9499         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9500         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9501         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
9502         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9503         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9504         final Acceleration bz2 = new Acceleration(0.0,
9505                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9506         calibrator.getBiasZAsAcceleration(bz2);
9507         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9508         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9509         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9510         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9511         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9512         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9513         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9514         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9515         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9516         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9517         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9518         final double[] bias1 = calibrator.getBias();
9519         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9520         final double[] bias2 = new double[3];
9521         calibrator.getBias(bias2);
9522         assertArrayEquals(bias1, bias2, 0.0);
9523         final Matrix b1 = calibrator.getBiasAsMatrix();
9524         assertEquals(b1, ba);
9525         final Matrix b2 = new Matrix(3, 1);
9526         calibrator.getBiasAsMatrix(b2);
9527         assertEquals(b1, b2);
9528         final Matrix ma1 = calibrator.getInitialMa();
9529         assertEquals(ma1, new Matrix(3, 3));
9530         final Matrix ma2 = new Matrix(3, 3);
9531         calibrator.getInitialMa(ma2);
9532         assertEquals(ma1, ma2);
9533         assertNull(calibrator.getMeasurements());
9534         assertTrue(calibrator.isCommonAxisUsed());
9535         assertSame(calibrator.getListener(), this);
9536         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
9537         assertFalse(calibrator.isReady());
9538         assertFalse(calibrator.isRunning());
9539         assertNull(calibrator.getEstimatedMa());
9540         assertNull(calibrator.getEstimatedSx());
9541         assertNull(calibrator.getEstimatedSy());
9542         assertNull(calibrator.getEstimatedSz());
9543         assertNull(calibrator.getEstimatedMxy());
9544         assertNull(calibrator.getEstimatedMxz());
9545         assertNull(calibrator.getEstimatedMyx());
9546         assertNull(calibrator.getEstimatedMyz());
9547         assertNull(calibrator.getEstimatedMzx());
9548         assertNull(calibrator.getEstimatedMzy());
9549         assertNull(calibrator.getEstimatedCovariance());
9550         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9551         assertNotNull(calibrator.getGroundTruthGravityNorm());
9552         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9553         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9554         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9555                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9556         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9557         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9558         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9559 
9560         // Force IllegalArgumentException
9561         calibrator = null;
9562         try {
9563             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
9564                     -gravityNorm, true, biasX, biasY, biasZ,
9565                     this);
9566             fail("IllegalArgumentException expected but not thrown");
9567         } catch (final IllegalArgumentException ignore) {
9568         }
9569         assertNull(calibrator);
9570     }
9571 
9572     @Test
9573     public void testConstructor93() throws WrongSizeException {
9574         final Collection<StandardDeviationBodyKinematics> measurements =
9575                 Collections.emptyList();
9576 
9577         final Matrix ba = generateBa();
9578         final double biasX = ba.getElementAtIndex(0);
9579         final double biasY = ba.getElementAtIndex(1);
9580         final double biasZ = ba.getElementAtIndex(2);
9581 
9582         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9583         final double latitude = Math.toRadians(
9584                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9585         final double longitude = Math.toRadians(
9586                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9587         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9588         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9589         final NEDVelocity nedVelocity = new NEDVelocity();
9590         final ECEFPosition ecefPosition = new ECEFPosition();
9591         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9592         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9593                 ecefPosition, ecefVelocity);
9594         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9595                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9596         final double gravityNorm = gravity.getNorm();
9597 
9598         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
9599                 new KnownBiasAndGravityNormAccelerometerCalibrator(
9600                         gravityNorm, measurements,
9601                         true, biasX, biasY, biasZ);
9602 
9603         // check default values
9604         assertEquals(calibrator.getBiasX(), biasX, 0.0);
9605         assertEquals(calibrator.getBiasY(), biasY, 0.0);
9606         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
9607         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
9608         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9609         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9610         final Acceleration bx2 = new Acceleration(0.0,
9611                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9612         calibrator.getBiasXAsAcceleration(bx2);
9613         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9614         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9615         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
9616         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9617         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9618         final Acceleration by2 = new Acceleration(0.0,
9619                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9620         calibrator.getBiasYAsAcceleration(by2);
9621         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9622         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9623         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
9624         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9625         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9626         final Acceleration bz2 = new Acceleration(0.0,
9627                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9628         calibrator.getBiasZAsAcceleration(bz2);
9629         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9630         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9631         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9632         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9633         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9634         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9635         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9636         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9637         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9638         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9639         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9640         final double[] bias1 = calibrator.getBias();
9641         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9642         final double[] bias2 = new double[3];
9643         calibrator.getBias(bias2);
9644         assertArrayEquals(bias1, bias2, 0.0);
9645         final Matrix b1 = calibrator.getBiasAsMatrix();
9646         assertEquals(b1, ba);
9647         final Matrix b2 = new Matrix(3, 1);
9648         calibrator.getBiasAsMatrix(b2);
9649         assertEquals(b1, b2);
9650         final Matrix ma1 = calibrator.getInitialMa();
9651         assertEquals(ma1, new Matrix(3, 3));
9652         final Matrix ma2 = new Matrix(3, 3);
9653         calibrator.getInitialMa(ma2);
9654         assertEquals(ma1, ma2);
9655         assertSame(calibrator.getMeasurements(), measurements);
9656         assertTrue(calibrator.isCommonAxisUsed());
9657         assertNull(calibrator.getListener());
9658         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
9659         assertFalse(calibrator.isReady());
9660         assertFalse(calibrator.isRunning());
9661         assertNull(calibrator.getEstimatedMa());
9662         assertNull(calibrator.getEstimatedSx());
9663         assertNull(calibrator.getEstimatedSy());
9664         assertNull(calibrator.getEstimatedSz());
9665         assertNull(calibrator.getEstimatedMxy());
9666         assertNull(calibrator.getEstimatedMxz());
9667         assertNull(calibrator.getEstimatedMyx());
9668         assertNull(calibrator.getEstimatedMyz());
9669         assertNull(calibrator.getEstimatedMzx());
9670         assertNull(calibrator.getEstimatedMzy());
9671         assertNull(calibrator.getEstimatedCovariance());
9672         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9673         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9674         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9675         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9676                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9677         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9678         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9679         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9680 
9681         // Force IllegalArgumentException
9682         calibrator = null;
9683         try {
9684             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
9685                     -gravityNorm, measurements,
9686                     true, biasX, biasY, biasZ);
9687             fail("IllegalArgumentException expected but not thrown");
9688         } catch (final IllegalArgumentException ignore) {
9689         }
9690         assertNull(calibrator);
9691     }
9692 
9693     @Test
9694     public void testConstructor94() throws WrongSizeException {
9695         final Collection<StandardDeviationBodyKinematics> measurements =
9696                 Collections.emptyList();
9697 
9698         final Matrix ba = generateBa();
9699         final double biasX = ba.getElementAtIndex(0);
9700         final double biasY = ba.getElementAtIndex(1);
9701         final double biasZ = ba.getElementAtIndex(2);
9702 
9703         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9704         final double latitude = Math.toRadians(
9705                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9706         final double longitude = Math.toRadians(
9707                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9708         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9709         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9710         final NEDVelocity nedVelocity = new NEDVelocity();
9711         final ECEFPosition ecefPosition = new ECEFPosition();
9712         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9713         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9714                 ecefPosition, ecefVelocity);
9715         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9716                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9717         final double gravityNorm = gravity.getNorm();
9718 
9719         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
9720                 new KnownBiasAndGravityNormAccelerometerCalibrator(
9721                         gravityNorm, measurements,
9722                         true, biasX, biasY, biasZ, this);
9723 
9724         // check default values
9725         assertEquals(calibrator.getBiasX(), biasX, 0.0);
9726         assertEquals(calibrator.getBiasY(), biasY, 0.0);
9727         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
9728         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
9729         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9730         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9731         final Acceleration bx2 = new Acceleration(0.0,
9732                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9733         calibrator.getBiasXAsAcceleration(bx2);
9734         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9735         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9736         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
9737         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9738         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9739         final Acceleration by2 = new Acceleration(0.0,
9740                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9741         calibrator.getBiasYAsAcceleration(by2);
9742         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9743         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9744         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
9745         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9746         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9747         final Acceleration bz2 = new Acceleration(0.0,
9748                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9749         calibrator.getBiasZAsAcceleration(bz2);
9750         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9751         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9752         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9753         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9754         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9755         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9756         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9757         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9758         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9759         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9760         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9761         final double[] bias1 = calibrator.getBias();
9762         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9763         final double[] bias2 = new double[3];
9764         calibrator.getBias(bias2);
9765         assertArrayEquals(bias1, bias2, 0.0);
9766         final Matrix b1 = calibrator.getBiasAsMatrix();
9767         assertEquals(b1, ba);
9768         final Matrix b2 = new Matrix(3, 1);
9769         calibrator.getBiasAsMatrix(b2);
9770         assertEquals(b1, b2);
9771         final Matrix ma1 = calibrator.getInitialMa();
9772         assertEquals(ma1, new Matrix(3, 3));
9773         final Matrix ma2 = new Matrix(3, 3);
9774         calibrator.getInitialMa(ma2);
9775         assertEquals(ma1, ma2);
9776         assertSame(calibrator.getMeasurements(), measurements);
9777         assertTrue(calibrator.isCommonAxisUsed());
9778         assertSame(calibrator.getListener(), this);
9779         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
9780         assertFalse(calibrator.isReady());
9781         assertFalse(calibrator.isRunning());
9782         assertNull(calibrator.getEstimatedMa());
9783         assertNull(calibrator.getEstimatedSx());
9784         assertNull(calibrator.getEstimatedSy());
9785         assertNull(calibrator.getEstimatedSz());
9786         assertNull(calibrator.getEstimatedMxy());
9787         assertNull(calibrator.getEstimatedMxz());
9788         assertNull(calibrator.getEstimatedMyx());
9789         assertNull(calibrator.getEstimatedMyz());
9790         assertNull(calibrator.getEstimatedMzx());
9791         assertNull(calibrator.getEstimatedMzy());
9792         assertNull(calibrator.getEstimatedCovariance());
9793         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9794         assertNotNull(calibrator.getGroundTruthGravityNorm());
9795         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9796         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9797         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9798                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9799         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9800         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9801         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9802 
9803         // Force IllegalArgumentException
9804         calibrator = null;
9805         try {
9806             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
9807                     -gravityNorm, measurements,
9808                     true, biasX, biasY, biasZ, this);
9809             fail("IllegalArgumentException expected but not thrown");
9810         } catch (final IllegalArgumentException ignore) {
9811         }
9812         assertNull(calibrator);
9813     }
9814 
9815     @Test
9816     public void testConstructor95() throws WrongSizeException {
9817         final Matrix ba = generateBa();
9818         final double biasX = ba.getElementAtIndex(0);
9819         final double biasY = ba.getElementAtIndex(1);
9820         final double biasZ = ba.getElementAtIndex(2);
9821 
9822         final Acceleration bx = new Acceleration(biasX,
9823                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
9824         final Acceleration by = new Acceleration(biasY,
9825                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
9826         final Acceleration bz = new Acceleration(biasZ,
9827                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
9828 
9829         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9830         final double latitude = Math.toRadians(
9831                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9832         final double longitude = Math.toRadians(
9833                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9834         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9835         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9836         final NEDVelocity nedVelocity = new NEDVelocity();
9837         final ECEFPosition ecefPosition = new ECEFPosition();
9838         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9839         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9840                 ecefPosition, ecefVelocity);
9841         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9842                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9843         final double gravityNorm = gravity.getNorm();
9844 
9845         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
9846                 new KnownBiasAndGravityNormAccelerometerCalibrator(
9847                         gravityNorm, bx, by, bz);
9848 
9849         // check default values
9850         assertEquals(calibrator.getBiasX(), biasX, 0.0);
9851         assertEquals(calibrator.getBiasY(), biasY, 0.0);
9852         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
9853         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
9854         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9855         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9856         final Acceleration bx2 = new Acceleration(0.0,
9857                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9858         calibrator.getBiasXAsAcceleration(bx2);
9859         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9860         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9861         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
9862         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9863         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9864         final Acceleration by2 = new Acceleration(0.0,
9865                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9866         calibrator.getBiasYAsAcceleration(by2);
9867         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9868         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9869         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
9870         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9871         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9872         final Acceleration bz2 = new Acceleration(0.0,
9873                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9874         calibrator.getBiasZAsAcceleration(bz2);
9875         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9876         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9877         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9878         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9879         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9880         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9881         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9882         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9883         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9884         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9885         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9886         final double[] bias1 = calibrator.getBias();
9887         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9888         final double[] bias2 = new double[3];
9889         calibrator.getBias(bias2);
9890         assertArrayEquals(bias1, bias2, 0.0);
9891         final Matrix b1 = calibrator.getBiasAsMatrix();
9892         assertEquals(b1, ba);
9893         final Matrix b2 = new Matrix(3, 1);
9894         calibrator.getBiasAsMatrix(b2);
9895         assertEquals(b1, b2);
9896         final Matrix ma1 = calibrator.getInitialMa();
9897         assertEquals(ma1, new Matrix(3, 3));
9898         final Matrix ma2 = new Matrix(3, 3);
9899         calibrator.getInitialMa(ma2);
9900         assertEquals(ma1, ma2);
9901         assertNull(calibrator.getMeasurements());
9902         assertFalse(calibrator.isCommonAxisUsed());
9903         assertNull(calibrator.getListener());
9904         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9905         assertFalse(calibrator.isReady());
9906         assertFalse(calibrator.isRunning());
9907         assertNull(calibrator.getEstimatedMa());
9908         assertNull(calibrator.getEstimatedSx());
9909         assertNull(calibrator.getEstimatedSy());
9910         assertNull(calibrator.getEstimatedSz());
9911         assertNull(calibrator.getEstimatedMxy());
9912         assertNull(calibrator.getEstimatedMxz());
9913         assertNull(calibrator.getEstimatedMyx());
9914         assertNull(calibrator.getEstimatedMyz());
9915         assertNull(calibrator.getEstimatedMzx());
9916         assertNull(calibrator.getEstimatedMzy());
9917         assertNull(calibrator.getEstimatedCovariance());
9918         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9919         assertNotNull(calibrator.getGroundTruthGravityNorm());
9920         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9921         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9922         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9923                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9924         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9925         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9926         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9927 
9928         // Force IllegalArgumentException
9929         calibrator = null;
9930         try {
9931             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
9932                     -gravityNorm, bx, by, bz);
9933             fail("IllegalArgumentException expected but not thrown");
9934         } catch (final IllegalArgumentException ignore) {
9935         }
9936         assertNull(calibrator);
9937     }
9938 
9939     @Test
9940     public void testConstructor96() throws WrongSizeException {
9941         final Matrix ba = generateBa();
9942         final double biasX = ba.getElementAtIndex(0);
9943         final double biasY = ba.getElementAtIndex(1);
9944         final double biasZ = ba.getElementAtIndex(2);
9945 
9946         final Acceleration bx = new Acceleration(biasX,
9947                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
9948         final Acceleration by = new Acceleration(biasY,
9949                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
9950         final Acceleration bz = new Acceleration(biasZ,
9951                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
9952 
9953         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9954         final double latitude = Math.toRadians(
9955                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9956         final double longitude = Math.toRadians(
9957                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9958         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9959         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9960         final NEDVelocity nedVelocity = new NEDVelocity();
9961         final ECEFPosition ecefPosition = new ECEFPosition();
9962         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9963         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9964                 ecefPosition, ecefVelocity);
9965         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9966                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9967         final double gravityNorm = gravity.getNorm();
9968 
9969         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
9970                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
9971                         bx, by, bz, this);
9972 
9973         // check default values
9974         assertEquals(calibrator.getBiasX(), biasX, 0.0);
9975         assertEquals(calibrator.getBiasY(), biasY, 0.0);
9976         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
9977         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
9978         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9979         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9980         final Acceleration bx2 = new Acceleration(0.0,
9981                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9982         calibrator.getBiasXAsAcceleration(bx2);
9983         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9984         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9985         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
9986         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9987         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9988         final Acceleration by2 = new Acceleration(0.0,
9989                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9990         calibrator.getBiasYAsAcceleration(by2);
9991         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9992         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9993         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
9994         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9995         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9996         final Acceleration bz2 = new Acceleration(0.0,
9997                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9998         calibrator.getBiasZAsAcceleration(bz2);
9999         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10000         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10001         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10002         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10003         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10004         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10005         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10006         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10007         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10008         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10009         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10010         final double[] bias1 = calibrator.getBias();
10011         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10012         final double[] bias2 = new double[3];
10013         calibrator.getBias(bias2);
10014         assertArrayEquals(bias1, bias2, 0.0);
10015         final Matrix b1 = calibrator.getBiasAsMatrix();
10016         assertEquals(b1, ba);
10017         final Matrix b2 = new Matrix(3, 1);
10018         calibrator.getBiasAsMatrix(b2);
10019         assertEquals(b1, b2);
10020         final Matrix ma1 = calibrator.getInitialMa();
10021         assertEquals(ma1, new Matrix(3, 3));
10022         final Matrix ma2 = new Matrix(3, 3);
10023         calibrator.getInitialMa(ma2);
10024         assertEquals(ma1, ma2);
10025         assertNull(calibrator.getMeasurements());
10026         assertFalse(calibrator.isCommonAxisUsed());
10027         assertSame(calibrator.getListener(), this);
10028         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10029         assertFalse(calibrator.isReady());
10030         assertFalse(calibrator.isRunning());
10031         assertNull(calibrator.getEstimatedMa());
10032         assertNull(calibrator.getEstimatedSx());
10033         assertNull(calibrator.getEstimatedSy());
10034         assertNull(calibrator.getEstimatedSz());
10035         assertNull(calibrator.getEstimatedMxy());
10036         assertNull(calibrator.getEstimatedMxz());
10037         assertNull(calibrator.getEstimatedMyx());
10038         assertNull(calibrator.getEstimatedMyz());
10039         assertNull(calibrator.getEstimatedMzx());
10040         assertNull(calibrator.getEstimatedMzy());
10041         assertNull(calibrator.getEstimatedCovariance());
10042         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10043         assertNotNull(calibrator.getGroundTruthGravityNorm());
10044         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10045         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10046         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10047                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10048         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10049         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10050         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10051 
10052         // Force IllegalArgumentException
10053         calibrator = null;
10054         try {
10055             calibrator =
10056                     new KnownBiasAndGravityNormAccelerometerCalibrator(
10057                             -gravityNorm, bx, by, bz, this);
10058             fail("IllegalArgumentException but not thrown");
10059         } catch (final IllegalArgumentException ignore) {
10060         }
10061         assertNull(calibrator);
10062     }
10063 
10064     @Test
10065     public void testConstructor97() throws WrongSizeException {
10066         final Collection<StandardDeviationBodyKinematics> measurements =
10067                 Collections.emptyList();
10068 
10069         final Matrix ba = generateBa();
10070         final double biasX = ba.getElementAtIndex(0);
10071         final double biasY = ba.getElementAtIndex(1);
10072         final double biasZ = ba.getElementAtIndex(2);
10073 
10074         final Acceleration bx = new Acceleration(biasX,
10075                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10076         final Acceleration by = new Acceleration(biasY,
10077                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10078         final Acceleration bz = new Acceleration(biasZ,
10079                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10080 
10081         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10082         final double latitude = Math.toRadians(
10083                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10084         final double longitude = Math.toRadians(
10085                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10086         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10087         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10088         final NEDVelocity nedVelocity = new NEDVelocity();
10089         final ECEFPosition ecefPosition = new ECEFPosition();
10090         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10091         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10092                 ecefPosition, ecefVelocity);
10093         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10094                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10095         final double gravityNorm = gravity.getNorm();
10096 
10097         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
10098                 new KnownBiasAndGravityNormAccelerometerCalibrator(
10099                         gravityNorm, measurements, bx, by, bz);
10100 
10101         // check default values
10102         assertEquals(calibrator.getBiasX(), biasX, 0.0);
10103         assertEquals(calibrator.getBiasY(), biasY, 0.0);
10104         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
10105         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
10106         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10107         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10108         final Acceleration bx2 = new Acceleration(0.0,
10109                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10110         calibrator.getBiasXAsAcceleration(bx2);
10111         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10112         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10113         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
10114         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10115         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10116         final Acceleration by2 = new Acceleration(0.0,
10117                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10118         calibrator.getBiasYAsAcceleration(by2);
10119         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10120         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10121         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
10122         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10123         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10124         final Acceleration bz2 = new Acceleration(0.0,
10125                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10126         calibrator.getBiasZAsAcceleration(bz2);
10127         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10128         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10129         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10130         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10131         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10132         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10133         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10134         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10135         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10136         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10137         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10138         final double[] bias1 = calibrator.getBias();
10139         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10140         final double[] bias2 = new double[3];
10141         calibrator.getBias(bias2);
10142         assertArrayEquals(bias1, bias2, 0.0);
10143         final Matrix b1 = calibrator.getBiasAsMatrix();
10144         assertEquals(b1, ba);
10145         final Matrix b2 = new Matrix(3, 1);
10146         calibrator.getBiasAsMatrix(b2);
10147         assertEquals(b1, b2);
10148         final Matrix ma1 = calibrator.getInitialMa();
10149         assertEquals(ma1, new Matrix(3, 3));
10150         final Matrix ma2 = new Matrix(3, 3);
10151         calibrator.getInitialMa(ma2);
10152         assertEquals(ma1, ma2);
10153         assertSame(calibrator.getMeasurements(), measurements);
10154         assertFalse(calibrator.isCommonAxisUsed());
10155         assertNull(calibrator.getListener());
10156         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10157         assertFalse(calibrator.isReady());
10158         assertFalse(calibrator.isRunning());
10159         assertNull(calibrator.getEstimatedMa());
10160         assertNull(calibrator.getEstimatedSx());
10161         assertNull(calibrator.getEstimatedSy());
10162         assertNull(calibrator.getEstimatedSz());
10163         assertNull(calibrator.getEstimatedMxy());
10164         assertNull(calibrator.getEstimatedMxz());
10165         assertNull(calibrator.getEstimatedMyx());
10166         assertNull(calibrator.getEstimatedMyz());
10167         assertNull(calibrator.getEstimatedMzx());
10168         assertNull(calibrator.getEstimatedMzy());
10169         assertNull(calibrator.getEstimatedCovariance());
10170         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10171         assertNotNull(calibrator.getGroundTruthGravityNorm());
10172         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10173         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10174         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10175                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10176         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10177         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10178         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10179 
10180         // Force IllegalArgumentException
10181         calibrator = null;
10182         try {
10183             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
10184                     -gravityNorm, measurements, bx, by, bz);
10185             fail("IllegalArgumentException expected but not thrown");
10186         } catch (final IllegalArgumentException ignore) {
10187         }
10188         assertNull(calibrator);
10189     }
10190 
10191     @Test
10192     public void testConstructor98() throws WrongSizeException {
10193         final Collection<StandardDeviationBodyKinematics> measurements =
10194                 Collections.emptyList();
10195 
10196         final Matrix ba = generateBa();
10197         final double biasX = ba.getElementAtIndex(0);
10198         final double biasY = ba.getElementAtIndex(1);
10199         final double biasZ = ba.getElementAtIndex(2);
10200 
10201         final Acceleration bx = new Acceleration(biasX,
10202                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10203         final Acceleration by = new Acceleration(biasY,
10204                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10205         final Acceleration bz = new Acceleration(biasZ,
10206                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10207 
10208         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10209         final double latitude = Math.toRadians(
10210                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10211         final double longitude = Math.toRadians(
10212                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10213         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10214         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10215         final NEDVelocity nedVelocity = new NEDVelocity();
10216         final ECEFPosition ecefPosition = new ECEFPosition();
10217         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10218         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10219                 ecefPosition, ecefVelocity);
10220         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10221                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10222         final double gravityNorm = gravity.getNorm();
10223 
10224         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
10225                 new KnownBiasAndGravityNormAccelerometerCalibrator(
10226                         gravityNorm, measurements,
10227                         bx, by, bz, this);
10228 
10229         // check default values
10230         assertEquals(calibrator.getBiasX(), biasX, 0.0);
10231         assertEquals(calibrator.getBiasY(), biasY, 0.0);
10232         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
10233         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
10234         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10235         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10236         final Acceleration bx2 = new Acceleration(0.0,
10237                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10238         calibrator.getBiasXAsAcceleration(bx2);
10239         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10240         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10241         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
10242         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10243         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10244         final Acceleration by2 = new Acceleration(0.0,
10245                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10246         calibrator.getBiasYAsAcceleration(by2);
10247         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10248         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10249         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
10250         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10251         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10252         final Acceleration bz2 = new Acceleration(0.0,
10253                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10254         calibrator.getBiasZAsAcceleration(bz2);
10255         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10256         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10257         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10258         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10259         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10260         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10261         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10262         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10263         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10264         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10265         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10266         final double[] bias1 = calibrator.getBias();
10267         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10268         final double[] bias2 = new double[3];
10269         calibrator.getBias(bias2);
10270         assertArrayEquals(bias1, bias2, 0.0);
10271         final Matrix b1 = calibrator.getBiasAsMatrix();
10272         assertEquals(b1, ba);
10273         final Matrix b2 = new Matrix(3, 1);
10274         calibrator.getBiasAsMatrix(b2);
10275         assertEquals(b1, b2);
10276         final Matrix ma1 = calibrator.getInitialMa();
10277         assertEquals(ma1, new Matrix(3, 3));
10278         final Matrix ma2 = new Matrix(3, 3);
10279         calibrator.getInitialMa(ma2);
10280         assertEquals(ma1, ma2);
10281         assertSame(calibrator.getMeasurements(), measurements);
10282         assertFalse(calibrator.isCommonAxisUsed());
10283         assertSame(calibrator.getListener(), this);
10284         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10285         assertFalse(calibrator.isReady());
10286         assertFalse(calibrator.isRunning());
10287         assertNull(calibrator.getEstimatedMa());
10288         assertNull(calibrator.getEstimatedSx());
10289         assertNull(calibrator.getEstimatedSy());
10290         assertNull(calibrator.getEstimatedSz());
10291         assertNull(calibrator.getEstimatedMxy());
10292         assertNull(calibrator.getEstimatedMxz());
10293         assertNull(calibrator.getEstimatedMyx());
10294         assertNull(calibrator.getEstimatedMyz());
10295         assertNull(calibrator.getEstimatedMzx());
10296         assertNull(calibrator.getEstimatedMzy());
10297         assertNull(calibrator.getEstimatedCovariance());
10298         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10299         assertNotNull(calibrator.getGroundTruthGravityNorm());
10300         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10301         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10302         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10303                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10304         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10305         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10306         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10307 
10308         // Force IllegalArgumentException
10309         calibrator = null;
10310         try {
10311             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
10312                     -gravityNorm, measurements,
10313                     bx, by, bz, this);
10314             fail("IllegalArgumentException expected but not thrown");
10315         } catch (final IllegalArgumentException ignore) {
10316         }
10317         assertNull(calibrator);
10318     }
10319 
10320     @Test
10321     public void testConstructor99() throws WrongSizeException {
10322         final Matrix ba = generateBa();
10323         final double biasX = ba.getElementAtIndex(0);
10324         final double biasY = ba.getElementAtIndex(1);
10325         final double biasZ = ba.getElementAtIndex(2);
10326 
10327         final Acceleration bx = new Acceleration(biasX,
10328                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10329         final Acceleration by = new Acceleration(biasY,
10330                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10331         final Acceleration bz = new Acceleration(biasZ,
10332                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10333 
10334         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10335         final double latitude = Math.toRadians(
10336                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10337         final double longitude = Math.toRadians(
10338                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10339         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10340         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10341         final NEDVelocity nedVelocity = new NEDVelocity();
10342         final ECEFPosition ecefPosition = new ECEFPosition();
10343         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10344         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10345                 ecefPosition, ecefVelocity);
10346         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10347                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10348         final double gravityNorm = gravity.getNorm();
10349 
10350         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
10351                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
10352                         true, bx, by, bz);
10353 
10354         // check default values
10355         assertEquals(calibrator.getBiasX(), biasX, 0.0);
10356         assertEquals(calibrator.getBiasY(), biasY, 0.0);
10357         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
10358         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
10359         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10360         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10361         final Acceleration bx2 = new Acceleration(0.0,
10362                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10363         calibrator.getBiasXAsAcceleration(bx2);
10364         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10365         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10366         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
10367         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10368         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10369         final Acceleration by2 = new Acceleration(0.0,
10370                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10371         calibrator.getBiasYAsAcceleration(by2);
10372         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10373         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10374         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
10375         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10376         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10377         final Acceleration bz2 = new Acceleration(0.0,
10378                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10379         calibrator.getBiasZAsAcceleration(bz2);
10380         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10381         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10382         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10383         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10384         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10385         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10386         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10387         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10388         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10389         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10390         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10391         final double[] bias1 = calibrator.getBias();
10392         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10393         final double[] bias2 = new double[3];
10394         calibrator.getBias(bias2);
10395         assertArrayEquals(bias1, bias2, 0.0);
10396         final Matrix b1 = calibrator.getBiasAsMatrix();
10397         assertEquals(b1, ba);
10398         final Matrix b2 = new Matrix(3, 1);
10399         calibrator.getBiasAsMatrix(b2);
10400         assertEquals(b1, b2);
10401         final Matrix ma1 = calibrator.getInitialMa();
10402         assertEquals(ma1, new Matrix(3, 3));
10403         final Matrix ma2 = new Matrix(3, 3);
10404         calibrator.getInitialMa(ma2);
10405         assertEquals(ma1, ma2);
10406         assertNull(calibrator.getMeasurements());
10407         assertTrue(calibrator.isCommonAxisUsed());
10408         assertNull(calibrator.getListener());
10409         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
10410         assertFalse(calibrator.isReady());
10411         assertFalse(calibrator.isRunning());
10412         assertNull(calibrator.getEstimatedMa());
10413         assertNull(calibrator.getEstimatedSx());
10414         assertNull(calibrator.getEstimatedSy());
10415         assertNull(calibrator.getEstimatedSz());
10416         assertNull(calibrator.getEstimatedMxy());
10417         assertNull(calibrator.getEstimatedMxz());
10418         assertNull(calibrator.getEstimatedMyx());
10419         assertNull(calibrator.getEstimatedMyz());
10420         assertNull(calibrator.getEstimatedMzx());
10421         assertNull(calibrator.getEstimatedMzy());
10422         assertNull(calibrator.getEstimatedCovariance());
10423         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10424         assertNotNull(calibrator.getGroundTruthGravityNorm());
10425         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10426         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10427         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10428                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10429         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10430         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10431         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10432 
10433         // Force IllegalArgumentException
10434         calibrator = null;
10435         try {
10436             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
10437                     -gravityNorm, true, bx, by, bz);
10438             fail("IllegalArgumentException expected but not thrown");
10439         } catch (final IllegalArgumentException ignore) {
10440         }
10441         assertNull(calibrator);
10442     }
10443 
10444     @Test
10445     public void testConstructor100() throws WrongSizeException {
10446         final Matrix ba = generateBa();
10447         final double biasX = ba.getElementAtIndex(0);
10448         final double biasY = ba.getElementAtIndex(1);
10449         final double biasZ = ba.getElementAtIndex(2);
10450 
10451         final Acceleration bx = new Acceleration(biasX,
10452                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10453         final Acceleration by = new Acceleration(biasY,
10454                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10455         final Acceleration bz = new Acceleration(biasZ,
10456                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10457 
10458         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10459         final double latitude = Math.toRadians(
10460                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10461         final double longitude = Math.toRadians(
10462                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10463         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10464         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10465         final NEDVelocity nedVelocity = new NEDVelocity();
10466         final ECEFPosition ecefPosition = new ECEFPosition();
10467         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10468         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10469                 ecefPosition, ecefVelocity);
10470         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10471                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10472         final double gravityNorm = gravity.getNorm();
10473 
10474         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
10475                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
10476                         true, bx, by, bz, this);
10477 
10478         // check default values
10479         assertEquals(calibrator.getBiasX(), biasX, 0.0);
10480         assertEquals(calibrator.getBiasY(), biasY, 0.0);
10481         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
10482         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
10483         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10484         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10485         final Acceleration bx2 = new Acceleration(0.0,
10486                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10487         calibrator.getBiasXAsAcceleration(bx2);
10488         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10489         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10490         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
10491         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10492         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10493         final Acceleration by2 = new Acceleration(0.0,
10494                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10495         calibrator.getBiasYAsAcceleration(by2);
10496         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10497         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10498         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
10499         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10500         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10501         final Acceleration bz2 = new Acceleration(0.0,
10502                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10503         calibrator.getBiasZAsAcceleration(bz2);
10504         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10505         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10506         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10507         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10508         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10509         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10510         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10511         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10512         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10513         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10514         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10515         final double[] bias1 = calibrator.getBias();
10516         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10517         final double[] bias2 = new double[3];
10518         calibrator.getBias(bias2);
10519         assertArrayEquals(bias1, bias2, 0.0);
10520         final Matrix b1 = calibrator.getBiasAsMatrix();
10521         assertEquals(b1, ba);
10522         final Matrix b2 = new Matrix(3, 1);
10523         calibrator.getBiasAsMatrix(b2);
10524         assertEquals(b1, b2);
10525         final Matrix ma1 = calibrator.getInitialMa();
10526         assertEquals(ma1, new Matrix(3, 3));
10527         final Matrix ma2 = new Matrix(3, 3);
10528         calibrator.getInitialMa(ma2);
10529         assertEquals(ma1, ma2);
10530         assertNull(calibrator.getMeasurements());
10531         assertTrue(calibrator.isCommonAxisUsed());
10532         assertSame(calibrator.getListener(), this);
10533         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
10534         assertFalse(calibrator.isReady());
10535         assertFalse(calibrator.isRunning());
10536         assertNull(calibrator.getEstimatedMa());
10537         assertNull(calibrator.getEstimatedSx());
10538         assertNull(calibrator.getEstimatedSy());
10539         assertNull(calibrator.getEstimatedSz());
10540         assertNull(calibrator.getEstimatedMxy());
10541         assertNull(calibrator.getEstimatedMxz());
10542         assertNull(calibrator.getEstimatedMyx());
10543         assertNull(calibrator.getEstimatedMyz());
10544         assertNull(calibrator.getEstimatedMzx());
10545         assertNull(calibrator.getEstimatedMzy());
10546         assertNull(calibrator.getEstimatedCovariance());
10547         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10548         assertNotNull(calibrator.getGroundTruthGravityNorm());
10549         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10550         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10551         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10552                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10553         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10554         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10555         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10556 
10557         // Force IllegalArgumentException
10558         calibrator = null;
10559         try {
10560             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
10561                     -gravityNorm, true, bx, by, bz, this);
10562             fail("IllegalArgumentException expected but not thrown");
10563         } catch (final IllegalArgumentException ignore) {
10564         }
10565         assertNull(calibrator);
10566     }
10567 
10568     @Test
10569     public void testConstructor101() throws WrongSizeException {
10570         final Collection<StandardDeviationBodyKinematics> measurements =
10571                 Collections.emptyList();
10572 
10573         final Matrix ba = generateBa();
10574         final double biasX = ba.getElementAtIndex(0);
10575         final double biasY = ba.getElementAtIndex(1);
10576         final double biasZ = ba.getElementAtIndex(2);
10577 
10578         final Acceleration bx = new Acceleration(biasX,
10579                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10580         final Acceleration by = new Acceleration(biasY,
10581                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10582         final Acceleration bz = new Acceleration(biasZ,
10583                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10584 
10585         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10586         final double latitude = Math.toRadians(
10587                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10588         final double longitude = Math.toRadians(
10589                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10590         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10591         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10592         final NEDVelocity nedVelocity = new NEDVelocity();
10593         final ECEFPosition ecefPosition = new ECEFPosition();
10594         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10595         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10596                 ecefPosition, ecefVelocity);
10597         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10598                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10599         final double gravityNorm = gravity.getNorm();
10600 
10601         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
10602                 new KnownBiasAndGravityNormAccelerometerCalibrator(
10603                         gravityNorm, measurements,
10604                         true, bx, by, bz);
10605 
10606         // check default values
10607         assertEquals(calibrator.getBiasX(), biasX, 0.0);
10608         assertEquals(calibrator.getBiasY(), biasY, 0.0);
10609         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
10610         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
10611         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10612         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10613         final Acceleration bx2 = new Acceleration(0.0,
10614                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10615         calibrator.getBiasXAsAcceleration(bx2);
10616         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10617         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10618         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
10619         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10620         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10621         final Acceleration by2 = new Acceleration(0.0,
10622                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10623         calibrator.getBiasYAsAcceleration(by2);
10624         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10625         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10626         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
10627         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10628         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10629         final Acceleration bz2 = new Acceleration(0.0,
10630                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10631         calibrator.getBiasZAsAcceleration(bz2);
10632         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10633         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10634         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10635         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10636         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10637         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10638         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10639         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10640         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10641         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10642         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10643         final double[] bias1 = calibrator.getBias();
10644         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10645         final double[] bias2 = new double[3];
10646         calibrator.getBias(bias2);
10647         assertArrayEquals(bias1, bias2, 0.0);
10648         final Matrix b1 = calibrator.getBiasAsMatrix();
10649         assertEquals(b1, ba);
10650         final Matrix b2 = new Matrix(3, 1);
10651         calibrator.getBiasAsMatrix(b2);
10652         assertEquals(b1, b2);
10653         final Matrix ma1 = calibrator.getInitialMa();
10654         assertEquals(ma1, new Matrix(3, 3));
10655         final Matrix ma2 = new Matrix(3, 3);
10656         calibrator.getInitialMa(ma2);
10657         assertEquals(ma1, ma2);
10658         assertSame(calibrator.getMeasurements(), measurements);
10659         assertTrue(calibrator.isCommonAxisUsed());
10660         assertNull(calibrator.getListener());
10661         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
10662         assertFalse(calibrator.isReady());
10663         assertFalse(calibrator.isRunning());
10664         assertNull(calibrator.getEstimatedMa());
10665         assertNull(calibrator.getEstimatedSx());
10666         assertNull(calibrator.getEstimatedSy());
10667         assertNull(calibrator.getEstimatedSz());
10668         assertNull(calibrator.getEstimatedMxy());
10669         assertNull(calibrator.getEstimatedMxz());
10670         assertNull(calibrator.getEstimatedMyx());
10671         assertNull(calibrator.getEstimatedMyz());
10672         assertNull(calibrator.getEstimatedMzx());
10673         assertNull(calibrator.getEstimatedMzy());
10674         assertNull(calibrator.getEstimatedCovariance());
10675         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10676         assertNotNull(calibrator.getGroundTruthGravityNorm());
10677         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10678         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10679         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10680                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10681         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10682         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10683         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10684 
10685         // Force IllegalArgumentException
10686         calibrator = null;
10687         try {
10688             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
10689                     -gravityNorm, measurements,
10690                     true, bx, by, bz);
10691             fail("IllegalArgumentException expected but not thrown");
10692         } catch (final IllegalArgumentException ignore) {
10693         }
10694         assertNull(calibrator);
10695     }
10696 
10697     @Test
10698     public void testConstructor102() throws WrongSizeException {
10699         final Collection<StandardDeviationBodyKinematics> measurements =
10700                 Collections.emptyList();
10701 
10702         final Matrix ba = generateBa();
10703         final double biasX = ba.getElementAtIndex(0);
10704         final double biasY = ba.getElementAtIndex(1);
10705         final double biasZ = ba.getElementAtIndex(2);
10706 
10707         final Acceleration bx = new Acceleration(biasX,
10708                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10709         final Acceleration by = new Acceleration(biasY,
10710                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10711         final Acceleration bz = new Acceleration(biasZ,
10712                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10713 
10714         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10715         final double latitude = Math.toRadians(
10716                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10717         final double longitude = Math.toRadians(
10718                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10719         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10720         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10721         final NEDVelocity nedVelocity = new NEDVelocity();
10722         final ECEFPosition ecefPosition = new ECEFPosition();
10723         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10724         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10725                 ecefPosition, ecefVelocity);
10726         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10727                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10728         final double gravityNorm = gravity.getNorm();
10729 
10730         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
10731                 new KnownBiasAndGravityNormAccelerometerCalibrator(
10732                         gravityNorm, measurements,
10733                         true, bx, by, bz, this);
10734 
10735         // check default values
10736         assertEquals(calibrator.getBiasX(), biasX, 0.0);
10737         assertEquals(calibrator.getBiasY(), biasY, 0.0);
10738         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
10739         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
10740         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10741         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10742         final Acceleration bx2 = new Acceleration(0.0,
10743                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10744         calibrator.getBiasXAsAcceleration(bx2);
10745         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10746         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10747         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
10748         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10749         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10750         final Acceleration by2 = new Acceleration(0.0,
10751                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10752         calibrator.getBiasYAsAcceleration(by2);
10753         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10754         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10755         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
10756         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10757         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10758         final Acceleration bz2 = new Acceleration(0.0,
10759                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10760         calibrator.getBiasZAsAcceleration(bz2);
10761         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10762         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10763         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10764         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10765         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10766         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10767         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10768         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10769         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10770         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10771         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10772         final double[] bias1 = calibrator.getBias();
10773         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10774         final double[] bias2 = new double[3];
10775         calibrator.getBias(bias2);
10776         assertArrayEquals(bias1, bias2, 0.0);
10777         final Matrix b1 = calibrator.getBiasAsMatrix();
10778         assertEquals(b1, ba);
10779         final Matrix b2 = new Matrix(3, 1);
10780         calibrator.getBiasAsMatrix(b2);
10781         assertEquals(b1, b2);
10782         final Matrix ma1 = calibrator.getInitialMa();
10783         assertEquals(ma1, new Matrix(3, 3));
10784         final Matrix ma2 = new Matrix(3, 3);
10785         calibrator.getInitialMa(ma2);
10786         assertEquals(ma1, ma2);
10787         assertSame(calibrator.getMeasurements(), measurements);
10788         assertTrue(calibrator.isCommonAxisUsed());
10789         assertSame(calibrator.getListener(), this);
10790         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
10791         assertFalse(calibrator.isReady());
10792         assertFalse(calibrator.isRunning());
10793         assertNull(calibrator.getEstimatedMa());
10794         assertNull(calibrator.getEstimatedSx());
10795         assertNull(calibrator.getEstimatedSy());
10796         assertNull(calibrator.getEstimatedSz());
10797         assertNull(calibrator.getEstimatedMxy());
10798         assertNull(calibrator.getEstimatedMxz());
10799         assertNull(calibrator.getEstimatedMyx());
10800         assertNull(calibrator.getEstimatedMyz());
10801         assertNull(calibrator.getEstimatedMzx());
10802         assertNull(calibrator.getEstimatedMzy());
10803         assertNull(calibrator.getEstimatedCovariance());
10804         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10805         assertNotNull(calibrator.getGroundTruthGravityNorm());
10806         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10807         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10808         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10809                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10810         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10811         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10812         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10813 
10814         // Force IllegalArgumentException
10815         calibrator = null;
10816         try {
10817             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
10818                     -gravityNorm, measurements,
10819                     true, bx, by, bz, this);
10820             fail("IllegalArgumentException expected but not thrown");
10821         } catch (final IllegalArgumentException ignore) {
10822         }
10823         assertNull(calibrator);
10824     }
10825 
10826     @Test
10827     public void testConstructor103() throws WrongSizeException {
10828         final Matrix ba = generateBa();
10829         final double biasX = ba.getElementAtIndex(0);
10830         final double biasY = ba.getElementAtIndex(1);
10831         final double biasZ = ba.getElementAtIndex(2);
10832 
10833         final Matrix ma = generateMaCommonAxis();
10834         final double sx = ma.getElementAt(0, 0);
10835         final double sy = ma.getElementAt(1, 1);
10836         final double sz = ma.getElementAt(2, 2);
10837 
10838         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10839         final double latitude = Math.toRadians(
10840                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10841         final double longitude = Math.toRadians(
10842                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10843         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10844         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10845         final NEDVelocity nedVelocity = new NEDVelocity();
10846         final ECEFPosition ecefPosition = new ECEFPosition();
10847         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10848         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10849                 ecefPosition, ecefVelocity);
10850         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10851                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10852         final double gravityNorm = gravity.getNorm();
10853 
10854         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
10855                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
10856                         biasX, biasY, biasZ, sx, sy, sz);
10857 
10858         // check default values
10859         assertEquals(calibrator.getBiasX(), biasX, 0.0);
10860         assertEquals(calibrator.getBiasY(), biasY, 0.0);
10861         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
10862         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
10863         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10864         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10865         final Acceleration bx2 = new Acceleration(0.0,
10866                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10867         calibrator.getBiasXAsAcceleration(bx2);
10868         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10869         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10870         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
10871         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10872         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10873         final Acceleration by2 = new Acceleration(0.0,
10874                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10875         calibrator.getBiasYAsAcceleration(by2);
10876         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10877         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10878         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
10879         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10880         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10881         final Acceleration bz2 = new Acceleration(0.0,
10882                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10883         calibrator.getBiasZAsAcceleration(bz2);
10884         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10885         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10886         assertEquals(calibrator.getInitialSx(), sx, 0.0);
10887         assertEquals(calibrator.getInitialSy(), sy, 0.0);
10888         assertEquals(calibrator.getInitialSz(), sz, 0.0);
10889         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10890         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10891         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10892         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10893         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10894         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10895         final double[] bias1 = calibrator.getBias();
10896         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10897         final double[] bias2 = new double[3];
10898         calibrator.getBias(bias2);
10899         assertArrayEquals(bias1, bias2, 0.0);
10900         final Matrix b1 = calibrator.getBiasAsMatrix();
10901         assertEquals(b1, ba);
10902         final Matrix b2 = new Matrix(3, 1);
10903         calibrator.getBiasAsMatrix(b2);
10904         assertEquals(b1, b2);
10905         final Matrix ma1 = calibrator.getInitialMa();
10906         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
10907         final Matrix ma2 = new Matrix(3, 3);
10908         calibrator.getInitialMa(ma2);
10909         assertEquals(ma1, ma2);
10910         assertNull(calibrator.getMeasurements());
10911         assertFalse(calibrator.isCommonAxisUsed());
10912         assertNull(calibrator.getListener());
10913         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10914         assertFalse(calibrator.isReady());
10915         assertFalse(calibrator.isRunning());
10916         assertNull(calibrator.getEstimatedMa());
10917         assertNull(calibrator.getEstimatedSx());
10918         assertNull(calibrator.getEstimatedSy());
10919         assertNull(calibrator.getEstimatedSz());
10920         assertNull(calibrator.getEstimatedMxy());
10921         assertNull(calibrator.getEstimatedMxz());
10922         assertNull(calibrator.getEstimatedMyx());
10923         assertNull(calibrator.getEstimatedMyz());
10924         assertNull(calibrator.getEstimatedMzx());
10925         assertNull(calibrator.getEstimatedMzy());
10926         assertNull(calibrator.getEstimatedCovariance());
10927         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10928         assertNotNull(calibrator.getGroundTruthGravityNorm());
10929         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10930         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10931         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10932                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10933         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10934         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10935         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10936 
10937         // Force IllegalArgumentException
10938         calibrator = null;
10939         try {
10940             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
10941                     -gravityNorm, biasX, biasY, biasZ, sx, sy, sz);
10942             fail("IllegalArgumentException expected but not thrown");
10943         } catch (final IllegalArgumentException ignore) {
10944         }
10945         assertNull(calibrator);
10946     }
10947 
10948     @Test
10949     public void testConstructor104() throws WrongSizeException {
10950         final Collection<StandardDeviationBodyKinematics> measurements =
10951                 Collections.emptyList();
10952 
10953         final Matrix ba = generateBa();
10954         final double biasX = ba.getElementAtIndex(0);
10955         final double biasY = ba.getElementAtIndex(1);
10956         final double biasZ = ba.getElementAtIndex(2);
10957 
10958         final Matrix ma = generateMaCommonAxis();
10959         final double sx = ma.getElementAt(0, 0);
10960         final double sy = ma.getElementAt(1, 1);
10961         final double sz = ma.getElementAt(2, 2);
10962 
10963         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10964         final double latitude = Math.toRadians(
10965                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10966         final double longitude = Math.toRadians(
10967                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10968         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10969         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10970         final NEDVelocity nedVelocity = new NEDVelocity();
10971         final ECEFPosition ecefPosition = new ECEFPosition();
10972         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10973         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10974                 ecefPosition, ecefVelocity);
10975         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10976                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10977         final double gravityNorm = gravity.getNorm();
10978 
10979         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
10980                 new KnownBiasAndGravityNormAccelerometerCalibrator(
10981                         gravityNorm, measurements,
10982                         biasX, biasY, biasZ, sx, sy, sz);
10983 
10984         // check default values
10985         assertEquals(calibrator.getBiasX(), biasX, 0.0);
10986         assertEquals(calibrator.getBiasY(), biasY, 0.0);
10987         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
10988         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
10989         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10990         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10991         final Acceleration bx2 = new Acceleration(0.0,
10992                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10993         calibrator.getBiasXAsAcceleration(bx2);
10994         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10995         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10996         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
10997         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10998         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10999         final Acceleration by2 = new Acceleration(0.0,
11000                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11001         calibrator.getBiasYAsAcceleration(by2);
11002         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11003         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11004         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
11005         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11006         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11007         final Acceleration bz2 = new Acceleration(0.0,
11008                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11009         calibrator.getBiasZAsAcceleration(bz2);
11010         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11011         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11012         assertEquals(calibrator.getInitialSx(), sx, 0.0);
11013         assertEquals(calibrator.getInitialSy(), sy, 0.0);
11014         assertEquals(calibrator.getInitialSz(), sz, 0.0);
11015         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11016         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11017         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11018         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11019         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11020         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11021         final double[] bias1 = calibrator.getBias();
11022         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11023         final double[] bias2 = new double[3];
11024         calibrator.getBias(bias2);
11025         assertArrayEquals(bias1, bias2, 0.0);
11026         final Matrix b1 = calibrator.getBiasAsMatrix();
11027         assertEquals(b1, ba);
11028         final Matrix b2 = new Matrix(3, 1);
11029         calibrator.getBiasAsMatrix(b2);
11030         assertEquals(b1, b2);
11031         final Matrix ma1 = calibrator.getInitialMa();
11032         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11033         final Matrix ma2 = new Matrix(3, 3);
11034         calibrator.getInitialMa(ma2);
11035         assertEquals(ma1, ma2);
11036         assertSame(calibrator.getMeasurements(), measurements);
11037         assertFalse(calibrator.isCommonAxisUsed());
11038         assertNull(calibrator.getListener());
11039         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11040         assertFalse(calibrator.isReady());
11041         assertFalse(calibrator.isRunning());
11042         assertNull(calibrator.getEstimatedMa());
11043         assertNull(calibrator.getEstimatedSx());
11044         assertNull(calibrator.getEstimatedSy());
11045         assertNull(calibrator.getEstimatedSz());
11046         assertNull(calibrator.getEstimatedMxy());
11047         assertNull(calibrator.getEstimatedMxz());
11048         assertNull(calibrator.getEstimatedMyx());
11049         assertNull(calibrator.getEstimatedMyz());
11050         assertNull(calibrator.getEstimatedMzx());
11051         assertNull(calibrator.getEstimatedMzy());
11052         assertNull(calibrator.getEstimatedCovariance());
11053         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11054         assertNotNull(calibrator.getGroundTruthGravityNorm());
11055         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11056         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11057         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11058                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11059         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11060         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11061         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11062 
11063         // Force IllegalArgumentException
11064         calibrator = null;
11065         try {
11066             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
11067                     -gravityNorm, measurements,
11068                     biasX, biasY, biasZ, sx, sy, sz);
11069             fail("IllegalArgumentException expected but not thrown");
11070         } catch (final IllegalArgumentException ignore) {
11071         }
11072         assertNull(calibrator);
11073     }
11074 
11075     @Test
11076     public void testConstructor105() throws WrongSizeException {
11077         final Collection<StandardDeviationBodyKinematics> measurements =
11078                 Collections.emptyList();
11079 
11080         final Matrix ba = generateBa();
11081         final double biasX = ba.getElementAtIndex(0);
11082         final double biasY = ba.getElementAtIndex(1);
11083         final double biasZ = ba.getElementAtIndex(2);
11084 
11085         final Matrix ma = generateMaCommonAxis();
11086         final double sx = ma.getElementAt(0, 0);
11087         final double sy = ma.getElementAt(1, 1);
11088         final double sz = ma.getElementAt(2, 2);
11089 
11090         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11091         final double latitude = Math.toRadians(
11092                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11093         final double longitude = Math.toRadians(
11094                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11095         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11096         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11097         final NEDVelocity nedVelocity = new NEDVelocity();
11098         final ECEFPosition ecefPosition = new ECEFPosition();
11099         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11100         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11101                 ecefPosition, ecefVelocity);
11102         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11103                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11104         final double gravityNorm = gravity.getNorm();
11105 
11106         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
11107                 new KnownBiasAndGravityNormAccelerometerCalibrator(
11108                         gravityNorm, measurements,
11109                         biasX, biasY, biasZ, sx, sy, sz, this);
11110 
11111         // check default values
11112         assertEquals(calibrator.getBiasX(), biasX, 0.0);
11113         assertEquals(calibrator.getBiasY(), biasY, 0.0);
11114         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
11115         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
11116         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11117         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11118         final Acceleration bx2 = new Acceleration(0.0,
11119                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11120         calibrator.getBiasXAsAcceleration(bx2);
11121         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11122         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11123         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
11124         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11125         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11126         final Acceleration by2 = new Acceleration(0.0,
11127                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11128         calibrator.getBiasYAsAcceleration(by2);
11129         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11130         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11131         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
11132         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11133         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11134         final Acceleration bz2 = new Acceleration(0.0,
11135                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11136         calibrator.getBiasZAsAcceleration(bz2);
11137         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11138         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11139         assertEquals(calibrator.getInitialSx(), sx, 0.0);
11140         assertEquals(calibrator.getInitialSy(), sy, 0.0);
11141         assertEquals(calibrator.getInitialSz(), sz, 0.0);
11142         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11143         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11144         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11145         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11146         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11147         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11148         final double[] bias1 = calibrator.getBias();
11149         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11150         final double[] bias2 = new double[3];
11151         calibrator.getBias(bias2);
11152         assertArrayEquals(bias1, bias2, 0.0);
11153         final Matrix b1 = calibrator.getBiasAsMatrix();
11154         assertEquals(b1, ba);
11155         final Matrix b2 = new Matrix(3, 1);
11156         calibrator.getBiasAsMatrix(b2);
11157         assertEquals(b1, b2);
11158         final Matrix ma1 = calibrator.getInitialMa();
11159         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11160         final Matrix ma2 = new Matrix(3, 3);
11161         calibrator.getInitialMa(ma2);
11162         assertEquals(ma1, ma2);
11163         assertSame(calibrator.getMeasurements(), measurements);
11164         assertFalse(calibrator.isCommonAxisUsed());
11165         assertSame(calibrator.getListener(), this);
11166         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11167         assertFalse(calibrator.isReady());
11168         assertFalse(calibrator.isRunning());
11169         assertNull(calibrator.getEstimatedMa());
11170         assertNull(calibrator.getEstimatedSx());
11171         assertNull(calibrator.getEstimatedSy());
11172         assertNull(calibrator.getEstimatedSz());
11173         assertNull(calibrator.getEstimatedMxy());
11174         assertNull(calibrator.getEstimatedMxz());
11175         assertNull(calibrator.getEstimatedMyx());
11176         assertNull(calibrator.getEstimatedMyz());
11177         assertNull(calibrator.getEstimatedMzx());
11178         assertNull(calibrator.getEstimatedMzy());
11179         assertNull(calibrator.getEstimatedCovariance());
11180         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11181         assertNotNull(calibrator.getGroundTruthGravityNorm());
11182         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11183         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11184         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11185                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11186         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11187         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11188         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11189 
11190         // Force IllegalArgumentException
11191         calibrator = null;
11192         try {
11193             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
11194                     -gravityNorm, measurements,
11195                     biasX, biasY, biasZ, sx, sy, sz, this);
11196             fail("IllegalArgumentException expected but not thrown");
11197         } catch (final IllegalArgumentException ignore) {
11198         }
11199         assertNull(calibrator);
11200     }
11201 
11202     @Test
11203     public void testConstructor106() throws WrongSizeException {
11204         final Matrix ba = generateBa();
11205         final double biasX = ba.getElementAtIndex(0);
11206         final double biasY = ba.getElementAtIndex(1);
11207         final double biasZ = ba.getElementAtIndex(2);
11208 
11209         final Matrix ma = generateMaCommonAxis();
11210         final double sx = ma.getElementAt(0, 0);
11211         final double sy = ma.getElementAt(1, 1);
11212         final double sz = ma.getElementAt(2, 2);
11213 
11214         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11215         final double latitude = Math.toRadians(
11216                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11217         final double longitude = Math.toRadians(
11218                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11219         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11220         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11221         final NEDVelocity nedVelocity = new NEDVelocity();
11222         final ECEFPosition ecefPosition = new ECEFPosition();
11223         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11224         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11225                 ecefPosition, ecefVelocity);
11226         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11227                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11228         final double gravityNorm = gravity.getNorm();
11229 
11230         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
11231                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
11232                         true, biasX, biasY, biasZ,
11233                         sx, sy, sz);
11234 
11235         // check default values
11236         assertEquals(calibrator.getBiasX(), biasX, 0.0);
11237         assertEquals(calibrator.getBiasY(), biasY, 0.0);
11238         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
11239         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
11240         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11241         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11242         final Acceleration bx2 = new Acceleration(0.0,
11243                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11244         calibrator.getBiasXAsAcceleration(bx2);
11245         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11246         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11247         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
11248         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11249         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11250         final Acceleration by2 = new Acceleration(0.0,
11251                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11252         calibrator.getBiasYAsAcceleration(by2);
11253         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11254         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11255         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
11256         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11257         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11258         final Acceleration bz2 = new Acceleration(0.0,
11259                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11260         calibrator.getBiasZAsAcceleration(bz2);
11261         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11262         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11263         assertEquals(calibrator.getInitialSx(), sx, 0.0);
11264         assertEquals(calibrator.getInitialSy(), sy, 0.0);
11265         assertEquals(calibrator.getInitialSz(), sz, 0.0);
11266         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11267         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11268         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11269         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11270         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11271         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11272         final double[] bias1 = calibrator.getBias();
11273         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11274         final double[] bias2 = new double[3];
11275         calibrator.getBias(bias2);
11276         assertArrayEquals(bias1, bias2, 0.0);
11277         final Matrix b1 = calibrator.getBiasAsMatrix();
11278         assertEquals(b1, ba);
11279         final Matrix b2 = new Matrix(3, 1);
11280         calibrator.getBiasAsMatrix(b2);
11281         assertEquals(b1, b2);
11282         final Matrix ma1 = calibrator.getInitialMa();
11283         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11284         final Matrix ma2 = new Matrix(3, 3);
11285         calibrator.getInitialMa(ma2);
11286         assertEquals(ma1, ma2);
11287         assertNull(calibrator.getMeasurements());
11288         assertTrue(calibrator.isCommonAxisUsed());
11289         assertNull(calibrator.getListener());
11290         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
11291         assertFalse(calibrator.isReady());
11292         assertFalse(calibrator.isRunning());
11293         assertNull(calibrator.getEstimatedMa());
11294         assertNull(calibrator.getEstimatedSx());
11295         assertNull(calibrator.getEstimatedSy());
11296         assertNull(calibrator.getEstimatedSz());
11297         assertNull(calibrator.getEstimatedMxy());
11298         assertNull(calibrator.getEstimatedMxz());
11299         assertNull(calibrator.getEstimatedMyx());
11300         assertNull(calibrator.getEstimatedMyz());
11301         assertNull(calibrator.getEstimatedMzx());
11302         assertNull(calibrator.getEstimatedMzy());
11303         assertNull(calibrator.getEstimatedCovariance());
11304         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11305         assertNotNull(calibrator.getGroundTruthGravityNorm());
11306         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11307         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11308         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11309                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11310         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11311         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11312         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11313 
11314         // Force IllegalArgumentException
11315         calibrator = null;
11316         try {
11317             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
11318                     -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz);
11319             fail("IllegalArgumentException expected but not thrown");
11320         } catch (final IllegalArgumentException ignore) {
11321         }
11322         assertNull(calibrator);
11323     }
11324 
11325     @Test
11326     public void testConstructor107() throws WrongSizeException {
11327         final Matrix ba = generateBa();
11328         final double biasX = ba.getElementAtIndex(0);
11329         final double biasY = ba.getElementAtIndex(1);
11330         final double biasZ = ba.getElementAtIndex(2);
11331 
11332         final Matrix ma = generateMaCommonAxis();
11333         final double sx = ma.getElementAt(0, 0);
11334         final double sy = ma.getElementAt(1, 1);
11335         final double sz = ma.getElementAt(2, 2);
11336 
11337         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11338         final double latitude = Math.toRadians(
11339                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11340         final double longitude = Math.toRadians(
11341                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11342         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11343         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11344         final NEDVelocity nedVelocity = new NEDVelocity();
11345         final ECEFPosition ecefPosition = new ECEFPosition();
11346         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11347         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11348                 ecefPosition, ecefVelocity);
11349         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11350                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11351         final double gravityNorm = gravity.getNorm();
11352 
11353         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
11354                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
11355                         true, biasX, biasY, biasZ, sx, sy, sz,
11356                         this);
11357 
11358         // check default values
11359         assertEquals(calibrator.getBiasX(), biasX, 0.0);
11360         assertEquals(calibrator.getBiasY(), biasY, 0.0);
11361         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
11362         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
11363         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11364         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11365         final Acceleration bx2 = new Acceleration(0.0,
11366                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11367         calibrator.getBiasXAsAcceleration(bx2);
11368         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11369         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11370         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
11371         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11372         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11373         final Acceleration by2 = new Acceleration(0.0,
11374                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11375         calibrator.getBiasYAsAcceleration(by2);
11376         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11377         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11378         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
11379         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11380         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11381         final Acceleration bz2 = new Acceleration(0.0,
11382                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11383         calibrator.getBiasZAsAcceleration(bz2);
11384         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11385         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11386         assertEquals(calibrator.getInitialSx(), sx, 0.0);
11387         assertEquals(calibrator.getInitialSy(), sy, 0.0);
11388         assertEquals(calibrator.getInitialSz(), sz, 0.0);
11389         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11390         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11391         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11392         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11393         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11394         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11395         final double[] bias1 = calibrator.getBias();
11396         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11397         final double[] bias2 = new double[3];
11398         calibrator.getBias(bias2);
11399         assertArrayEquals(bias1, bias2, 0.0);
11400         final Matrix b1 = calibrator.getBiasAsMatrix();
11401         assertEquals(b1, ba);
11402         final Matrix b2 = new Matrix(3, 1);
11403         calibrator.getBiasAsMatrix(b2);
11404         assertEquals(b1, b2);
11405         final Matrix ma1 = calibrator.getInitialMa();
11406         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11407         final Matrix ma2 = new Matrix(3, 3);
11408         calibrator.getInitialMa(ma2);
11409         assertEquals(ma1, ma2);
11410         assertNull(calibrator.getMeasurements());
11411         assertTrue(calibrator.isCommonAxisUsed());
11412         assertSame(calibrator.getListener(), this);
11413         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
11414         assertFalse(calibrator.isReady());
11415         assertFalse(calibrator.isRunning());
11416         assertNull(calibrator.getEstimatedMa());
11417         assertNull(calibrator.getEstimatedSx());
11418         assertNull(calibrator.getEstimatedSy());
11419         assertNull(calibrator.getEstimatedSz());
11420         assertNull(calibrator.getEstimatedMxy());
11421         assertNull(calibrator.getEstimatedMxz());
11422         assertNull(calibrator.getEstimatedMyx());
11423         assertNull(calibrator.getEstimatedMyz());
11424         assertNull(calibrator.getEstimatedMzx());
11425         assertNull(calibrator.getEstimatedMzy());
11426         assertNull(calibrator.getEstimatedCovariance());
11427         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11428         assertNotNull(calibrator.getGroundTruthGravityNorm());
11429         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11430         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11431         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11432                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11433         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11434         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11435         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11436 
11437         // Force IllegalArgumentException
11438         calibrator = null;
11439         try {
11440             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
11441                     -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz,
11442                     this);
11443             fail("IllegalArgumentException expected but not thrown");
11444         } catch (final IllegalArgumentException ignore) {
11445         }
11446         assertNull(calibrator);
11447     }
11448 
11449     @Test
11450     public void testConstructor108() throws WrongSizeException {
11451         final Collection<StandardDeviationBodyKinematics> measurements =
11452                 Collections.emptyList();
11453 
11454         final Matrix ba = generateBa();
11455         final double biasX = ba.getElementAtIndex(0);
11456         final double biasY = ba.getElementAtIndex(1);
11457         final double biasZ = ba.getElementAtIndex(2);
11458 
11459         final Matrix ma = generateMaCommonAxis();
11460         final double sx = ma.getElementAt(0, 0);
11461         final double sy = ma.getElementAt(1, 1);
11462         final double sz = ma.getElementAt(2, 2);
11463 
11464         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11465         final double latitude = Math.toRadians(
11466                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11467         final double longitude = Math.toRadians(
11468                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11469         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11470         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11471         final NEDVelocity nedVelocity = new NEDVelocity();
11472         final ECEFPosition ecefPosition = new ECEFPosition();
11473         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11474         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11475                 ecefPosition, ecefVelocity);
11476         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11477                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11478         final double gravityNorm = gravity.getNorm();
11479 
11480         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
11481                 new KnownBiasAndGravityNormAccelerometerCalibrator(
11482                         gravityNorm, measurements,
11483                         true, biasX, biasY, biasZ,
11484                         sx, sy, sz);
11485 
11486         // check default values
11487         assertEquals(calibrator.getBiasX(), biasX, 0.0);
11488         assertEquals(calibrator.getBiasY(), biasY, 0.0);
11489         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
11490         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
11491         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11492         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11493         final Acceleration bx2 = new Acceleration(0.0,
11494                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11495         calibrator.getBiasXAsAcceleration(bx2);
11496         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11497         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11498         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
11499         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11500         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11501         final Acceleration by2 = new Acceleration(0.0,
11502                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11503         calibrator.getBiasYAsAcceleration(by2);
11504         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11505         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11506         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
11507         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11508         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11509         final Acceleration bz2 = new Acceleration(0.0,
11510                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11511         calibrator.getBiasZAsAcceleration(bz2);
11512         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11513         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11514         assertEquals(calibrator.getInitialSx(), sx, 0.0);
11515         assertEquals(calibrator.getInitialSy(), sy, 0.0);
11516         assertEquals(calibrator.getInitialSz(), sz, 0.0);
11517         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11518         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11519         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11520         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11521         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11522         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11523         final double[] bias1 = calibrator.getBias();
11524         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11525         final double[] bias2 = new double[3];
11526         calibrator.getBias(bias2);
11527         assertArrayEquals(bias1, bias2, 0.0);
11528         final Matrix b1 = calibrator.getBiasAsMatrix();
11529         assertEquals(b1, ba);
11530         final Matrix b2 = new Matrix(3, 1);
11531         calibrator.getBiasAsMatrix(b2);
11532         assertEquals(b1, b2);
11533         final Matrix ma1 = calibrator.getInitialMa();
11534         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11535         final Matrix ma2 = new Matrix(3, 3);
11536         calibrator.getInitialMa(ma2);
11537         assertEquals(ma1, ma2);
11538         assertSame(calibrator.getMeasurements(), measurements);
11539         assertTrue(calibrator.isCommonAxisUsed());
11540         assertNull(calibrator.getListener());
11541         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
11542         assertFalse(calibrator.isReady());
11543         assertFalse(calibrator.isRunning());
11544         assertNull(calibrator.getEstimatedMa());
11545         assertNull(calibrator.getEstimatedSx());
11546         assertNull(calibrator.getEstimatedSy());
11547         assertNull(calibrator.getEstimatedSz());
11548         assertNull(calibrator.getEstimatedMxy());
11549         assertNull(calibrator.getEstimatedMxz());
11550         assertNull(calibrator.getEstimatedMyx());
11551         assertNull(calibrator.getEstimatedMyz());
11552         assertNull(calibrator.getEstimatedMzx());
11553         assertNull(calibrator.getEstimatedMzy());
11554         assertNull(calibrator.getEstimatedCovariance());
11555         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11556         assertNotNull(calibrator.getGroundTruthGravityNorm());
11557         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11558         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11559         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11560                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11561         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11562         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11563         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11564 
11565         // Force IllegalArgumentException
11566         calibrator = null;
11567         try {
11568             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
11569                     -gravityNorm, measurements,
11570                     true, biasX, biasY, biasZ,
11571                     sx, sy, sz);
11572             fail("IllegalArgumentException expected but not thrown");
11573         } catch (final IllegalArgumentException ignore) {
11574         }
11575         assertNull(calibrator);
11576     }
11577 
11578     @Test
11579     public void testConstructor109() throws WrongSizeException {
11580         final Collection<StandardDeviationBodyKinematics> measurements =
11581                 Collections.emptyList();
11582 
11583         final Matrix ba = generateBa();
11584         final double biasX = ba.getElementAtIndex(0);
11585         final double biasY = ba.getElementAtIndex(1);
11586         final double biasZ = ba.getElementAtIndex(2);
11587 
11588         final Matrix ma = generateMaCommonAxis();
11589         final double sx = ma.getElementAt(0, 0);
11590         final double sy = ma.getElementAt(1, 1);
11591         final double sz = ma.getElementAt(2, 2);
11592 
11593         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11594         final double latitude = Math.toRadians(
11595                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11596         final double longitude = Math.toRadians(
11597                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11598         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11599         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11600         final NEDVelocity nedVelocity = new NEDVelocity();
11601         final ECEFPosition ecefPosition = new ECEFPosition();
11602         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11603         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11604                 ecefPosition, ecefVelocity);
11605         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11606                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11607         final double gravityNorm = gravity.getNorm();
11608 
11609         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
11610                 new KnownBiasAndGravityNormAccelerometerCalibrator(
11611                         gravityNorm, measurements,
11612                         true, biasX, biasY, biasZ, sx, sy, sz,
11613                         this);
11614 
11615         // check default values
11616         assertEquals(calibrator.getBiasX(), biasX, 0.0);
11617         assertEquals(calibrator.getBiasY(), biasY, 0.0);
11618         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
11619         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
11620         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11621         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11622         final Acceleration bx2 = new Acceleration(0.0,
11623                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11624         calibrator.getBiasXAsAcceleration(bx2);
11625         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11626         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11627         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
11628         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11629         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11630         final Acceleration by2 = new Acceleration(0.0,
11631                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11632         calibrator.getBiasYAsAcceleration(by2);
11633         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11634         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11635         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
11636         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11637         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11638         final Acceleration bz2 = new Acceleration(0.0,
11639                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11640         calibrator.getBiasZAsAcceleration(bz2);
11641         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11642         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11643         assertEquals(calibrator.getInitialSx(), sx, 0.0);
11644         assertEquals(calibrator.getInitialSy(), sy, 0.0);
11645         assertEquals(calibrator.getInitialSz(), sz, 0.0);
11646         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11647         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11648         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11649         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11650         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11651         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11652         final double[] bias1 = calibrator.getBias();
11653         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11654         final double[] bias2 = new double[3];
11655         calibrator.getBias(bias2);
11656         assertArrayEquals(bias1, bias2, 0.0);
11657         final Matrix b1 = calibrator.getBiasAsMatrix();
11658         assertEquals(b1, ba);
11659         final Matrix b2 = new Matrix(3, 1);
11660         calibrator.getBiasAsMatrix(b2);
11661         assertEquals(b1, b2);
11662         final Matrix ma1 = calibrator.getInitialMa();
11663         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11664         final Matrix ma2 = new Matrix(3, 3);
11665         calibrator.getInitialMa(ma2);
11666         assertEquals(ma1, ma2);
11667         assertSame(calibrator.getMeasurements(), measurements);
11668         assertTrue(calibrator.isCommonAxisUsed());
11669         assertSame(calibrator.getListener(), this);
11670         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
11671         assertFalse(calibrator.isReady());
11672         assertFalse(calibrator.isRunning());
11673         assertNull(calibrator.getEstimatedMa());
11674         assertNull(calibrator.getEstimatedSx());
11675         assertNull(calibrator.getEstimatedSy());
11676         assertNull(calibrator.getEstimatedSz());
11677         assertNull(calibrator.getEstimatedMxy());
11678         assertNull(calibrator.getEstimatedMxz());
11679         assertNull(calibrator.getEstimatedMyx());
11680         assertNull(calibrator.getEstimatedMyz());
11681         assertNull(calibrator.getEstimatedMzx());
11682         assertNull(calibrator.getEstimatedMzy());
11683         assertNull(calibrator.getEstimatedCovariance());
11684         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11685         assertNotNull(calibrator.getGroundTruthGravityNorm());
11686         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11687         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11688         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11689                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11690         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11691         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11692         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11693 
11694         // Force IllegalArgumentException
11695         calibrator = null;
11696         try {
11697             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
11698                     -gravityNorm, measurements,
11699                     true, biasX, biasY, biasZ, sx, sy, sz,
11700                     this);
11701             fail("IllegalArgumentException expected but not thrown");
11702         } catch (final IllegalArgumentException ignore) {
11703         }
11704         assertNull(calibrator);
11705     }
11706 
11707     @Test
11708     public void testConstructor110() throws WrongSizeException {
11709         final Matrix ba = generateBa();
11710         final double biasX = ba.getElementAtIndex(0);
11711         final double biasY = ba.getElementAtIndex(1);
11712         final double biasZ = ba.getElementAtIndex(2);
11713 
11714         final Acceleration bx = new Acceleration(biasX,
11715                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11716         final Acceleration by = new Acceleration(biasY,
11717                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11718         final Acceleration bz = new Acceleration(biasZ,
11719                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11720 
11721         final Matrix ma = generateMaCommonAxis();
11722         final double sx = ma.getElementAt(0, 0);
11723         final double sy = ma.getElementAt(1, 1);
11724         final double sz = ma.getElementAt(2, 2);
11725 
11726         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11727         final double latitude = Math.toRadians(
11728                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11729         final double longitude = Math.toRadians(
11730                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11731         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11732         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11733         final NEDVelocity nedVelocity = new NEDVelocity();
11734         final ECEFPosition ecefPosition = new ECEFPosition();
11735         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11736         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11737                 ecefPosition, ecefVelocity);
11738         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11739                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11740         final double gravityNorm = gravity.getNorm();
11741 
11742         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
11743                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
11744                         bx, by, bz, sx, sy, sz);
11745 
11746         // check default values
11747         assertEquals(calibrator.getBiasX(), biasX, 0.0);
11748         assertEquals(calibrator.getBiasY(), biasY, 0.0);
11749         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
11750         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
11751         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11752         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11753         final Acceleration bx2 = new Acceleration(0.0,
11754                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11755         calibrator.getBiasXAsAcceleration(bx2);
11756         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11757         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11758         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
11759         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11760         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11761         final Acceleration by2 = new Acceleration(0.0,
11762                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11763         calibrator.getBiasYAsAcceleration(by2);
11764         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11765         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11766         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
11767         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11768         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11769         final Acceleration bz2 = new Acceleration(0.0,
11770                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11771         calibrator.getBiasZAsAcceleration(bz2);
11772         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11773         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11774         assertEquals(calibrator.getInitialSx(), sx, 0.0);
11775         assertEquals(calibrator.getInitialSy(), sy, 0.0);
11776         assertEquals(calibrator.getInitialSz(), sz, 0.0);
11777         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11778         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11779         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11780         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11781         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11782         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11783         final double[] bias1 = calibrator.getBias();
11784         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11785         final double[] bias2 = new double[3];
11786         calibrator.getBias(bias2);
11787         assertArrayEquals(bias1, bias2, 0.0);
11788         final Matrix b1 = calibrator.getBiasAsMatrix();
11789         assertEquals(b1, ba);
11790         final Matrix b2 = new Matrix(3, 1);
11791         calibrator.getBiasAsMatrix(b2);
11792         assertEquals(b1, b2);
11793         final Matrix ma1 = calibrator.getInitialMa();
11794         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11795         final Matrix ma2 = new Matrix(3, 3);
11796         calibrator.getInitialMa(ma2);
11797         assertEquals(ma1, ma2);
11798         assertNull(calibrator.getMeasurements());
11799         assertFalse(calibrator.isCommonAxisUsed());
11800         assertNull(calibrator.getListener());
11801         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11802         assertFalse(calibrator.isReady());
11803         assertFalse(calibrator.isRunning());
11804         assertNull(calibrator.getEstimatedMa());
11805         assertNull(calibrator.getEstimatedSx());
11806         assertNull(calibrator.getEstimatedSy());
11807         assertNull(calibrator.getEstimatedSz());
11808         assertNull(calibrator.getEstimatedMxy());
11809         assertNull(calibrator.getEstimatedMxz());
11810         assertNull(calibrator.getEstimatedMyx());
11811         assertNull(calibrator.getEstimatedMyz());
11812         assertNull(calibrator.getEstimatedMzx());
11813         assertNull(calibrator.getEstimatedMzy());
11814         assertNull(calibrator.getEstimatedCovariance());
11815         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11816         assertNotNull(calibrator.getGroundTruthGravityNorm());
11817         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11818         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11819         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11820                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11821         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11822         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11823         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11824 
11825         // Force IllegalArgumentException
11826         calibrator = null;
11827         try {
11828             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
11829                     -gravityNorm, bx, by, bz, sx, sy, sz);
11830             fail("IllegalArgumentException expected but not thrown");
11831         } catch (final IllegalArgumentException ignore) {
11832         }
11833         assertNull(calibrator);
11834     }
11835 
11836     @Test
11837     public void testConstructor111() throws WrongSizeException {
11838         final Matrix ba = generateBa();
11839         final double biasX = ba.getElementAtIndex(0);
11840         final double biasY = ba.getElementAtIndex(1);
11841         final double biasZ = ba.getElementAtIndex(2);
11842 
11843         final Acceleration bx = new Acceleration(biasX,
11844                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11845         final Acceleration by = new Acceleration(biasY,
11846                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11847         final Acceleration bz = new Acceleration(biasZ,
11848                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11849 
11850         final Matrix ma = generateMaCommonAxis();
11851         final double sx = ma.getElementAt(0, 0);
11852         final double sy = ma.getElementAt(1, 1);
11853         final double sz = ma.getElementAt(2, 2);
11854 
11855         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11856         final double latitude = Math.toRadians(
11857                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11858         final double longitude = Math.toRadians(
11859                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11860         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11861         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11862         final NEDVelocity nedVelocity = new NEDVelocity();
11863         final ECEFPosition ecefPosition = new ECEFPosition();
11864         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11865         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11866                 ecefPosition, ecefVelocity);
11867         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11868                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11869         final double gravityNorm = gravity.getNorm();
11870 
11871         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
11872                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
11873                         bx, by, bz, sx, sy, sz, this);
11874 
11875         // check default values
11876         assertEquals(calibrator.getBiasX(), biasX, 0.0);
11877         assertEquals(calibrator.getBiasY(), biasY, 0.0);
11878         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
11879         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
11880         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11881         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11882         final Acceleration bx2 = new Acceleration(0.0,
11883                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11884         calibrator.getBiasXAsAcceleration(bx2);
11885         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11886         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11887         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
11888         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11889         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11890         final Acceleration by2 = new Acceleration(0.0,
11891                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11892         calibrator.getBiasYAsAcceleration(by2);
11893         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11894         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11895         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
11896         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11897         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11898         final Acceleration bz2 = new Acceleration(0.0,
11899                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11900         calibrator.getBiasZAsAcceleration(bz2);
11901         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11902         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11903         assertEquals(calibrator.getInitialSx(), sx, 0.0);
11904         assertEquals(calibrator.getInitialSy(), sy, 0.0);
11905         assertEquals(calibrator.getInitialSz(), sz, 0.0);
11906         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11907         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11908         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11909         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11910         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11911         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11912         final double[] bias1 = calibrator.getBias();
11913         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11914         final double[] bias2 = new double[3];
11915         calibrator.getBias(bias2);
11916         assertArrayEquals(bias1, bias2, 0.0);
11917         final Matrix b1 = calibrator.getBiasAsMatrix();
11918         assertEquals(b1, ba);
11919         final Matrix b2 = new Matrix(3, 1);
11920         calibrator.getBiasAsMatrix(b2);
11921         assertEquals(b1, b2);
11922         final Matrix ma1 = calibrator.getInitialMa();
11923         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11924         final Matrix ma2 = new Matrix(3, 3);
11925         calibrator.getInitialMa(ma2);
11926         assertEquals(ma1, ma2);
11927         assertNull(calibrator.getMeasurements());
11928         assertFalse(calibrator.isCommonAxisUsed());
11929         assertSame(calibrator.getListener(), this);
11930         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11931         assertFalse(calibrator.isReady());
11932         assertFalse(calibrator.isRunning());
11933         assertNull(calibrator.getEstimatedMa());
11934         assertNull(calibrator.getEstimatedSx());
11935         assertNull(calibrator.getEstimatedSy());
11936         assertNull(calibrator.getEstimatedSz());
11937         assertNull(calibrator.getEstimatedMxy());
11938         assertNull(calibrator.getEstimatedMxz());
11939         assertNull(calibrator.getEstimatedMyx());
11940         assertNull(calibrator.getEstimatedMyz());
11941         assertNull(calibrator.getEstimatedMzx());
11942         assertNull(calibrator.getEstimatedMzy());
11943         assertNull(calibrator.getEstimatedCovariance());
11944         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11945         assertNotNull(calibrator.getGroundTruthGravityNorm());
11946         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11947         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11948         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11949                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11950         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11951         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11952         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11953 
11954         // Force IllegalArgumentException
11955         calibrator = null;
11956         try {
11957             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
11958                     -gravityNorm, bx, by, bz, sx, sy, sz, this);
11959             fail("IllegalArgumentException expected but not thrown");
11960         } catch (final IllegalArgumentException ignore) {
11961         }
11962         assertNull(calibrator);
11963     }
11964 
11965     @Test
11966     public void testConstructor112() throws WrongSizeException {
11967         final Collection<StandardDeviationBodyKinematics> measurements =
11968                 Collections.emptyList();
11969 
11970         final Matrix ba = generateBa();
11971         final double biasX = ba.getElementAtIndex(0);
11972         final double biasY = ba.getElementAtIndex(1);
11973         final double biasZ = ba.getElementAtIndex(2);
11974 
11975         final Acceleration bx = new Acceleration(biasX,
11976                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11977         final Acceleration by = new Acceleration(biasY,
11978                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11979         final Acceleration bz = new Acceleration(biasZ,
11980                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11981 
11982         final Matrix ma = generateMaCommonAxis();
11983         final double sx = ma.getElementAt(0, 0);
11984         final double sy = ma.getElementAt(1, 1);
11985         final double sz = ma.getElementAt(2, 2);
11986 
11987         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11988         final double latitude = Math.toRadians(
11989                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11990         final double longitude = Math.toRadians(
11991                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11992         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11993         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11994         final NEDVelocity nedVelocity = new NEDVelocity();
11995         final ECEFPosition ecefPosition = new ECEFPosition();
11996         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11997         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11998                 ecefPosition, ecefVelocity);
11999         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12000                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12001         final double gravityNorm = gravity.getNorm();
12002 
12003         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
12004                 new KnownBiasAndGravityNormAccelerometerCalibrator(
12005                         gravityNorm, measurements,
12006                         bx, by, bz, sx, sy, sz);
12007 
12008         // check default values
12009         assertEquals(calibrator.getBiasX(), biasX, 0.0);
12010         assertEquals(calibrator.getBiasY(), biasY, 0.0);
12011         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
12012         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
12013         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12014         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12015         final Acceleration bx2 = new Acceleration(0.0,
12016                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12017         calibrator.getBiasXAsAcceleration(bx2);
12018         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12019         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12020         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
12021         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12022         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12023         final Acceleration by2 = new Acceleration(0.0,
12024                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12025         calibrator.getBiasYAsAcceleration(by2);
12026         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12027         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12028         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
12029         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12030         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12031         final Acceleration bz2 = new Acceleration(0.0,
12032                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12033         calibrator.getBiasZAsAcceleration(bz2);
12034         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12035         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12036         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12037         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12038         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12039         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12040         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12041         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12042         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12043         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12044         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12045         final double[] bias1 = calibrator.getBias();
12046         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12047         final double[] bias2 = new double[3];
12048         calibrator.getBias(bias2);
12049         assertArrayEquals(bias1, bias2, 0.0);
12050         final Matrix b1 = calibrator.getBiasAsMatrix();
12051         assertEquals(b1, ba);
12052         final Matrix b2 = new Matrix(3, 1);
12053         calibrator.getBiasAsMatrix(b2);
12054         assertEquals(b1, b2);
12055         final Matrix ma1 = calibrator.getInitialMa();
12056         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12057         final Matrix ma2 = new Matrix(3, 3);
12058         calibrator.getInitialMa(ma2);
12059         assertEquals(ma1, ma2);
12060         assertSame(calibrator.getMeasurements(), measurements);
12061         assertFalse(calibrator.isCommonAxisUsed());
12062         assertNull(calibrator.getListener());
12063         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12064         assertFalse(calibrator.isReady());
12065         assertFalse(calibrator.isRunning());
12066         assertNull(calibrator.getEstimatedMa());
12067         assertNull(calibrator.getEstimatedSx());
12068         assertNull(calibrator.getEstimatedSy());
12069         assertNull(calibrator.getEstimatedSz());
12070         assertNull(calibrator.getEstimatedMxy());
12071         assertNull(calibrator.getEstimatedMxz());
12072         assertNull(calibrator.getEstimatedMyx());
12073         assertNull(calibrator.getEstimatedMyz());
12074         assertNull(calibrator.getEstimatedMzx());
12075         assertNull(calibrator.getEstimatedMzy());
12076         assertNull(calibrator.getEstimatedCovariance());
12077         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12078         assertNotNull(calibrator.getGroundTruthGravityNorm());
12079         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12080         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12081         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12082                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12083         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12084         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12085         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12086 
12087         // Force IllegalArgumentException
12088         calibrator = null;
12089         try {
12090             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
12091                     -gravityNorm, measurements,
12092                     bx, by, bz, sx, sy, sz);
12093             fail("IllegalArgumentException expected but not thrown");
12094         } catch (final IllegalArgumentException ignore) {
12095         }
12096         assertNull(calibrator);
12097     }
12098 
12099     @Test
12100     public void testConstructor113() throws WrongSizeException {
12101         final Collection<StandardDeviationBodyKinematics> measurements =
12102                 Collections.emptyList();
12103 
12104         final Matrix ba = generateBa();
12105         final double biasX = ba.getElementAtIndex(0);
12106         final double biasY = ba.getElementAtIndex(1);
12107         final double biasZ = ba.getElementAtIndex(2);
12108 
12109         final Acceleration bx = new Acceleration(biasX,
12110                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12111         final Acceleration by = new Acceleration(biasY,
12112                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12113         final Acceleration bz = new Acceleration(biasZ,
12114                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12115 
12116         final Matrix ma = generateMaCommonAxis();
12117         final double sx = ma.getElementAt(0, 0);
12118         final double sy = ma.getElementAt(1, 1);
12119         final double sz = ma.getElementAt(2, 2);
12120 
12121         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12122         final double latitude = Math.toRadians(
12123                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12124         final double longitude = Math.toRadians(
12125                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12126         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12127         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12128         final NEDVelocity nedVelocity = new NEDVelocity();
12129         final ECEFPosition ecefPosition = new ECEFPosition();
12130         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12131         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12132                 ecefPosition, ecefVelocity);
12133         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12134                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12135         final double gravityNorm = gravity.getNorm();
12136 
12137         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
12138                 new KnownBiasAndGravityNormAccelerometerCalibrator(
12139                         gravityNorm, measurements,
12140                         bx, by, bz, sx, sy, sz, this);
12141 
12142         // check default values
12143         assertEquals(calibrator.getBiasX(), biasX, 0.0);
12144         assertEquals(calibrator.getBiasY(), biasY, 0.0);
12145         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
12146         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
12147         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12148         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12149         final Acceleration bx2 = new Acceleration(0.0,
12150                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12151         calibrator.getBiasXAsAcceleration(bx2);
12152         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12153         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12154         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
12155         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12156         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12157         final Acceleration by2 = new Acceleration(0.0,
12158                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12159         calibrator.getBiasYAsAcceleration(by2);
12160         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12161         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12162         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
12163         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12164         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12165         final Acceleration bz2 = new Acceleration(0.0,
12166                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12167         calibrator.getBiasZAsAcceleration(bz2);
12168         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12169         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12170         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12171         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12172         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12173         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12174         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12175         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12176         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12177         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12178         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12179         final double[] bias1 = calibrator.getBias();
12180         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12181         final double[] bias2 = new double[3];
12182         calibrator.getBias(bias2);
12183         assertArrayEquals(bias1, bias2, 0.0);
12184         final Matrix b1 = calibrator.getBiasAsMatrix();
12185         assertEquals(b1, ba);
12186         final Matrix b2 = new Matrix(3, 1);
12187         calibrator.getBiasAsMatrix(b2);
12188         assertEquals(b1, b2);
12189         final Matrix ma1 = calibrator.getInitialMa();
12190         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12191         final Matrix ma2 = new Matrix(3, 3);
12192         calibrator.getInitialMa(ma2);
12193         assertEquals(ma1, ma2);
12194         assertSame(calibrator.getMeasurements(), measurements);
12195         assertFalse(calibrator.isCommonAxisUsed());
12196         assertSame(calibrator.getListener(), this);
12197         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12198         assertFalse(calibrator.isReady());
12199         assertFalse(calibrator.isRunning());
12200         assertNull(calibrator.getEstimatedMa());
12201         assertNull(calibrator.getEstimatedSx());
12202         assertNull(calibrator.getEstimatedSy());
12203         assertNull(calibrator.getEstimatedSz());
12204         assertNull(calibrator.getEstimatedMxy());
12205         assertNull(calibrator.getEstimatedMxz());
12206         assertNull(calibrator.getEstimatedMyx());
12207         assertNull(calibrator.getEstimatedMyz());
12208         assertNull(calibrator.getEstimatedMzx());
12209         assertNull(calibrator.getEstimatedMzy());
12210         assertNull(calibrator.getEstimatedCovariance());
12211         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12212         assertNotNull(calibrator.getGroundTruthGravityNorm());
12213         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12214         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12215         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12216                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12217         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12218         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12219         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12220 
12221         // Force IllegalArgumentException
12222         calibrator = null;
12223         try {
12224             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
12225                     -gravityNorm, measurements,
12226                     bx, by, bz, sx, sy, sz, this);
12227             fail("IllegalArgumentException expected but not thrown");
12228         } catch (final IllegalArgumentException ignore) {
12229         }
12230         assertNull(calibrator);
12231     }
12232 
12233     @Test
12234     public void testConstructor114() throws WrongSizeException {
12235         final Matrix ba = generateBa();
12236         final double biasX = ba.getElementAtIndex(0);
12237         final double biasY = ba.getElementAtIndex(1);
12238         final double biasZ = ba.getElementAtIndex(2);
12239 
12240         final Acceleration bx = new Acceleration(biasX,
12241                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12242         final Acceleration by = new Acceleration(biasY,
12243                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12244         final Acceleration bz = new Acceleration(biasZ,
12245                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12246 
12247         final Matrix ma = generateMaCommonAxis();
12248         final double sx = ma.getElementAt(0, 0);
12249         final double sy = ma.getElementAt(1, 1);
12250         final double sz = ma.getElementAt(2, 2);
12251 
12252         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12253         final double latitude = Math.toRadians(
12254                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12255         final double longitude = Math.toRadians(
12256                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12257         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12258         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12259         final NEDVelocity nedVelocity = new NEDVelocity();
12260         final ECEFPosition ecefPosition = new ECEFPosition();
12261         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12262         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12263                 ecefPosition, ecefVelocity);
12264         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12265                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12266         final double gravityNorm = gravity.getNorm();
12267 
12268         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
12269                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
12270                         true, bx, by, bz, sx, sy, sz);
12271 
12272         // check default values
12273         assertEquals(calibrator.getBiasX(), biasX, 0.0);
12274         assertEquals(calibrator.getBiasY(), biasY, 0.0);
12275         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
12276         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
12277         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12278         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12279         final Acceleration bx2 = new Acceleration(0.0,
12280                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12281         calibrator.getBiasXAsAcceleration(bx2);
12282         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12283         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12284         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
12285         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12286         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12287         final Acceleration by2 = new Acceleration(0.0,
12288                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12289         calibrator.getBiasYAsAcceleration(by2);
12290         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12291         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12292         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
12293         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12294         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12295         final Acceleration bz2 = new Acceleration(0.0,
12296                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12297         calibrator.getBiasZAsAcceleration(bz2);
12298         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12299         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12300         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12301         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12302         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12303         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12304         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12305         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12306         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12307         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12308         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12309         final double[] bias1 = calibrator.getBias();
12310         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12311         final double[] bias2 = new double[3];
12312         calibrator.getBias(bias2);
12313         assertArrayEquals(bias1, bias2, 0.0);
12314         final Matrix b1 = calibrator.getBiasAsMatrix();
12315         assertEquals(b1, ba);
12316         final Matrix b2 = new Matrix(3, 1);
12317         calibrator.getBiasAsMatrix(b2);
12318         assertEquals(b1, b2);
12319         final Matrix ma1 = calibrator.getInitialMa();
12320         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12321         final Matrix ma2 = new Matrix(3, 3);
12322         calibrator.getInitialMa(ma2);
12323         assertEquals(ma1, ma2);
12324         assertNull(calibrator.getMeasurements());
12325         assertTrue(calibrator.isCommonAxisUsed());
12326         assertNull(calibrator.getListener());
12327         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
12328         assertFalse(calibrator.isReady());
12329         assertFalse(calibrator.isRunning());
12330         assertNull(calibrator.getEstimatedMa());
12331         assertNull(calibrator.getEstimatedSx());
12332         assertNull(calibrator.getEstimatedSy());
12333         assertNull(calibrator.getEstimatedSz());
12334         assertNull(calibrator.getEstimatedMxy());
12335         assertNull(calibrator.getEstimatedMxz());
12336         assertNull(calibrator.getEstimatedMyx());
12337         assertNull(calibrator.getEstimatedMyz());
12338         assertNull(calibrator.getEstimatedMzx());
12339         assertNull(calibrator.getEstimatedMzy());
12340         assertNull(calibrator.getEstimatedCovariance());
12341         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12342         assertNotNull(calibrator.getGroundTruthGravityNorm());
12343         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12344         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12345         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12346                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12347         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12348         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12349         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12350 
12351         // Force IllegalArgumentException
12352         calibrator = null;
12353         try {
12354             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
12355                     -gravityNorm, true, bx, by, bz,
12356                     sx, sy, sz);
12357             fail("IllegalArgumentException expected but not thrown");
12358         } catch (final IllegalArgumentException ignore) {
12359         }
12360         assertNull(calibrator);
12361     }
12362 
12363     @Test
12364     public void testConstructor115() throws WrongSizeException {
12365         final Matrix ba = generateBa();
12366         final double biasX = ba.getElementAtIndex(0);
12367         final double biasY = ba.getElementAtIndex(1);
12368         final double biasZ = ba.getElementAtIndex(2);
12369 
12370         final Acceleration bx = new Acceleration(biasX,
12371                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12372         final Acceleration by = new Acceleration(biasY,
12373                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12374         final Acceleration bz = new Acceleration(biasZ,
12375                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12376 
12377         final Matrix ma = generateMaCommonAxis();
12378         final double sx = ma.getElementAt(0, 0);
12379         final double sy = ma.getElementAt(1, 1);
12380         final double sz = ma.getElementAt(2, 2);
12381 
12382         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12383         final double latitude = Math.toRadians(
12384                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12385         final double longitude = Math.toRadians(
12386                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12387         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12388         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12389         final NEDVelocity nedVelocity = new NEDVelocity();
12390         final ECEFPosition ecefPosition = new ECEFPosition();
12391         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12392         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12393                 ecefPosition, ecefVelocity);
12394         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12395                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12396         final double gravityNorm = gravity.getNorm();
12397 
12398         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
12399                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
12400                         true, bx, by, bz, sx, sy, sz,
12401                         this);
12402 
12403         // check default values
12404         assertEquals(calibrator.getBiasX(), biasX, 0.0);
12405         assertEquals(calibrator.getBiasY(), biasY, 0.0);
12406         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
12407         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
12408         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12409         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12410         final Acceleration bx2 = new Acceleration(0.0,
12411                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12412         calibrator.getBiasXAsAcceleration(bx2);
12413         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12414         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12415         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
12416         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12417         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12418         final Acceleration by2 = new Acceleration(0.0,
12419                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12420         calibrator.getBiasYAsAcceleration(by2);
12421         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12422         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12423         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
12424         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12425         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12426         final Acceleration bz2 = new Acceleration(0.0,
12427                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12428         calibrator.getBiasZAsAcceleration(bz2);
12429         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12430         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12431         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12432         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12433         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12434         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12435         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12436         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12437         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12438         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12439         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12440         final double[] bias1 = calibrator.getBias();
12441         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12442         final double[] bias2 = new double[3];
12443         calibrator.getBias(bias2);
12444         assertArrayEquals(bias1, bias2, 0.0);
12445         final Matrix b1 = calibrator.getBiasAsMatrix();
12446         assertEquals(b1, ba);
12447         final Matrix b2 = new Matrix(3, 1);
12448         calibrator.getBiasAsMatrix(b2);
12449         assertEquals(b1, b2);
12450         final Matrix ma1 = calibrator.getInitialMa();
12451         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12452         final Matrix ma2 = new Matrix(3, 3);
12453         calibrator.getInitialMa(ma2);
12454         assertEquals(ma1, ma2);
12455         assertNull(calibrator.getMeasurements());
12456         assertTrue(calibrator.isCommonAxisUsed());
12457         assertSame(calibrator.getListener(), this);
12458         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
12459         assertFalse(calibrator.isReady());
12460         assertFalse(calibrator.isRunning());
12461         assertNull(calibrator.getEstimatedMa());
12462         assertNull(calibrator.getEstimatedSx());
12463         assertNull(calibrator.getEstimatedSy());
12464         assertNull(calibrator.getEstimatedSz());
12465         assertNull(calibrator.getEstimatedMxy());
12466         assertNull(calibrator.getEstimatedMxz());
12467         assertNull(calibrator.getEstimatedMyx());
12468         assertNull(calibrator.getEstimatedMyz());
12469         assertNull(calibrator.getEstimatedMzx());
12470         assertNull(calibrator.getEstimatedMzy());
12471         assertNull(calibrator.getEstimatedCovariance());
12472         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12473         assertNotNull(calibrator.getGroundTruthGravityNorm());
12474         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12475         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12476         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12477                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12478         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12479         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12480         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12481 
12482         // Force IllegalArgumentException
12483         calibrator = null;
12484         try {
12485             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
12486                     -gravityNorm, true, bx, by, bz, sx, sy, sz,
12487                     this);
12488             fail("IllegalArgumentException expected but not thrown");
12489         } catch (final IllegalArgumentException ignore) {
12490         }
12491         assertNull(calibrator);
12492     }
12493 
12494     @Test
12495     public void testConstructor116() throws WrongSizeException {
12496         final Collection<StandardDeviationBodyKinematics> measurements =
12497                 Collections.emptyList();
12498 
12499         final Matrix ba = generateBa();
12500         final double biasX = ba.getElementAtIndex(0);
12501         final double biasY = ba.getElementAtIndex(1);
12502         final double biasZ = ba.getElementAtIndex(2);
12503 
12504         final Acceleration bx = new Acceleration(biasX,
12505                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12506         final Acceleration by = new Acceleration(biasY,
12507                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12508         final Acceleration bz = new Acceleration(biasZ,
12509                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12510 
12511         final Matrix ma = generateMaCommonAxis();
12512         final double sx = ma.getElementAt(0, 0);
12513         final double sy = ma.getElementAt(1, 1);
12514         final double sz = ma.getElementAt(2, 2);
12515 
12516         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12517         final double latitude = Math.toRadians(
12518                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12519         final double longitude = Math.toRadians(
12520                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12521         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12522         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12523         final NEDVelocity nedVelocity = new NEDVelocity();
12524         final ECEFPosition ecefPosition = new ECEFPosition();
12525         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12526         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12527                 ecefPosition, ecefVelocity);
12528         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12529                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12530         final double gravityNorm = gravity.getNorm();
12531 
12532         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
12533                 new KnownBiasAndGravityNormAccelerometerCalibrator(
12534                         gravityNorm, measurements,
12535                         true, bx, by, bz, sx, sy, sz);
12536 
12537         // check default values
12538         assertEquals(calibrator.getBiasX(), biasX, 0.0);
12539         assertEquals(calibrator.getBiasY(), biasY, 0.0);
12540         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
12541         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
12542         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12543         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12544         final Acceleration bx2 = new Acceleration(0.0,
12545                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12546         calibrator.getBiasXAsAcceleration(bx2);
12547         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12548         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12549         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
12550         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12551         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12552         final Acceleration by2 = new Acceleration(0.0,
12553                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12554         calibrator.getBiasYAsAcceleration(by2);
12555         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12556         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12557         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
12558         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12559         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12560         final Acceleration bz2 = new Acceleration(0.0,
12561                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12562         calibrator.getBiasZAsAcceleration(bz2);
12563         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12564         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12565         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12566         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12567         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12568         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12569         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12570         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12571         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12572         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12573         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12574         final double[] bias1 = calibrator.getBias();
12575         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12576         final double[] bias2 = new double[3];
12577         calibrator.getBias(bias2);
12578         assertArrayEquals(bias1, bias2, 0.0);
12579         final Matrix b1 = calibrator.getBiasAsMatrix();
12580         assertEquals(b1, ba);
12581         final Matrix b2 = new Matrix(3, 1);
12582         calibrator.getBiasAsMatrix(b2);
12583         assertEquals(b1, b2);
12584         final Matrix ma1 = calibrator.getInitialMa();
12585         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12586         final Matrix ma2 = new Matrix(3, 3);
12587         calibrator.getInitialMa(ma2);
12588         assertEquals(ma1, ma2);
12589         assertSame(calibrator.getMeasurements(), measurements);
12590         assertTrue(calibrator.isCommonAxisUsed());
12591         assertNull(calibrator.getListener());
12592         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
12593         assertFalse(calibrator.isReady());
12594         assertFalse(calibrator.isRunning());
12595         assertNull(calibrator.getEstimatedMa());
12596         assertNull(calibrator.getEstimatedSx());
12597         assertNull(calibrator.getEstimatedSy());
12598         assertNull(calibrator.getEstimatedSz());
12599         assertNull(calibrator.getEstimatedMxy());
12600         assertNull(calibrator.getEstimatedMxz());
12601         assertNull(calibrator.getEstimatedMyx());
12602         assertNull(calibrator.getEstimatedMyz());
12603         assertNull(calibrator.getEstimatedMzx());
12604         assertNull(calibrator.getEstimatedMzy());
12605         assertNull(calibrator.getEstimatedCovariance());
12606         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12607         assertNotNull(calibrator.getGroundTruthGravityNorm());
12608         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12609         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12610         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12611                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12612         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12613         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12614         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12615 
12616         // Force IllegalArgumentException
12617         calibrator = null;
12618         try {
12619             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
12620                     -gravityNorm, measurements,
12621                     true, bx, by, bz, sx, sy, sz);
12622             fail("IllegalArgumentException expected but not thrown");
12623         } catch (final IllegalArgumentException ignore) {
12624         }
12625         assertNull(calibrator);
12626     }
12627 
12628     @Test
12629     public void testConstructor117() throws WrongSizeException {
12630         final Collection<StandardDeviationBodyKinematics> measurements =
12631                 Collections.emptyList();
12632 
12633         final Matrix ba = generateBa();
12634         final double biasX = ba.getElementAtIndex(0);
12635         final double biasY = ba.getElementAtIndex(1);
12636         final double biasZ = ba.getElementAtIndex(2);
12637 
12638         final Acceleration bx = new Acceleration(biasX,
12639                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12640         final Acceleration by = new Acceleration(biasY,
12641                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12642         final Acceleration bz = new Acceleration(biasZ,
12643                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12644 
12645         final Matrix ma = generateMaCommonAxis();
12646         final double sx = ma.getElementAt(0, 0);
12647         final double sy = ma.getElementAt(1, 1);
12648         final double sz = ma.getElementAt(2, 2);
12649 
12650         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12651         final double latitude = Math.toRadians(
12652                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12653         final double longitude = Math.toRadians(
12654                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12655         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12656         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12657         final NEDVelocity nedVelocity = new NEDVelocity();
12658         final ECEFPosition ecefPosition = new ECEFPosition();
12659         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12660         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12661                 ecefPosition, ecefVelocity);
12662         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12663                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12664         final double gravityNorm = gravity.getNorm();
12665 
12666         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
12667                 new KnownBiasAndGravityNormAccelerometerCalibrator(
12668                         gravityNorm, measurements,
12669                         true, bx, by, bz, sx, sy, sz, this);
12670 
12671         // check default values
12672         assertEquals(calibrator.getBiasX(), biasX, 0.0);
12673         assertEquals(calibrator.getBiasY(), biasY, 0.0);
12674         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
12675         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
12676         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12677         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12678         final Acceleration bx2 = new Acceleration(0.0,
12679                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12680         calibrator.getBiasXAsAcceleration(bx2);
12681         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12682         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12683         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
12684         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12685         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12686         final Acceleration by2 = new Acceleration(0.0,
12687                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12688         calibrator.getBiasYAsAcceleration(by2);
12689         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12690         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12691         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
12692         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12693         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12694         final Acceleration bz2 = new Acceleration(0.0,
12695                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12696         calibrator.getBiasZAsAcceleration(bz2);
12697         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12698         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12699         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12700         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12701         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12702         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12703         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12704         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12705         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12706         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12707         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12708         final double[] bias1 = calibrator.getBias();
12709         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12710         final double[] bias2 = new double[3];
12711         calibrator.getBias(bias2);
12712         assertArrayEquals(bias1, bias2, 0.0);
12713         final Matrix b1 = calibrator.getBiasAsMatrix();
12714         assertEquals(b1, ba);
12715         final Matrix b2 = new Matrix(3, 1);
12716         calibrator.getBiasAsMatrix(b2);
12717         assertEquals(b1, b2);
12718         final Matrix ma1 = calibrator.getInitialMa();
12719         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12720         final Matrix ma2 = new Matrix(3, 3);
12721         calibrator.getInitialMa(ma2);
12722         assertEquals(ma1, ma2);
12723         assertSame(calibrator.getMeasurements(), measurements);
12724         assertTrue(calibrator.isCommonAxisUsed());
12725         assertSame(calibrator.getListener(), this);
12726         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
12727         assertFalse(calibrator.isReady());
12728         assertFalse(calibrator.isRunning());
12729         assertNull(calibrator.getEstimatedMa());
12730         assertNull(calibrator.getEstimatedSx());
12731         assertNull(calibrator.getEstimatedSy());
12732         assertNull(calibrator.getEstimatedSz());
12733         assertNull(calibrator.getEstimatedMxy());
12734         assertNull(calibrator.getEstimatedMxz());
12735         assertNull(calibrator.getEstimatedMyx());
12736         assertNull(calibrator.getEstimatedMyz());
12737         assertNull(calibrator.getEstimatedMzx());
12738         assertNull(calibrator.getEstimatedMzy());
12739         assertNull(calibrator.getEstimatedCovariance());
12740         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12741         assertNotNull(calibrator.getGroundTruthGravityNorm());
12742         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12743         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12744         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12745                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12746         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12747         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12748         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12749 
12750         // Force IllegalArgumentException
12751         calibrator = null;
12752         try {
12753             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
12754                     -gravityNorm, measurements,
12755                     true, bx, by, bz, sx, sy, sz, this);
12756             fail("IllegalArgumentException expected but not thrown");
12757         } catch (final IllegalArgumentException ignore) {
12758         }
12759         assertNull(calibrator);
12760     }
12761 
12762     @Test
12763     public void testConstructor118() throws WrongSizeException {
12764         final Matrix ba = generateBa();
12765         final double biasX = ba.getElementAtIndex(0);
12766         final double biasY = ba.getElementAtIndex(1);
12767         final double biasZ = ba.getElementAtIndex(2);
12768 
12769         final Matrix ma = generateMaCommonAxis();
12770         final double sx = ma.getElementAt(0, 0);
12771         final double sy = ma.getElementAt(1, 1);
12772         final double sz = ma.getElementAt(2, 2);
12773         final double mxy = ma.getElementAt(0, 1);
12774         final double mxz = ma.getElementAt(0, 2);
12775         final double myx = ma.getElementAt(1, 0);
12776         final double myz = ma.getElementAt(1, 2);
12777         final double mzx = ma.getElementAt(2, 0);
12778         final double mzy = ma.getElementAt(2, 1);
12779 
12780         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12781         final double latitude = Math.toRadians(
12782                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12783         final double longitude = Math.toRadians(
12784                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12785         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12786         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12787         final NEDVelocity nedVelocity = new NEDVelocity();
12788         final ECEFPosition ecefPosition = new ECEFPosition();
12789         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12790         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12791                 ecefPosition, ecefVelocity);
12792         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12793                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12794         final double gravityNorm = gravity.getNorm();
12795 
12796         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
12797                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
12798                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
12799                         myx, myz, mzx, mzy);
12800 
12801         // check default values
12802         assertEquals(calibrator.getBiasX(), biasX, 0.0);
12803         assertEquals(calibrator.getBiasY(), biasY, 0.0);
12804         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
12805         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
12806         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12807         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12808         final Acceleration bx2 = new Acceleration(0.0,
12809                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12810         calibrator.getBiasXAsAcceleration(bx2);
12811         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12812         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12813         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
12814         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12815         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12816         final Acceleration by2 = new Acceleration(0.0,
12817                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12818         calibrator.getBiasYAsAcceleration(by2);
12819         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12820         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12821         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
12822         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12823         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12824         final Acceleration bz2 = new Acceleration(0.0,
12825                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12826         calibrator.getBiasZAsAcceleration(bz2);
12827         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12828         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12829         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12830         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12831         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12832         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
12833         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
12834         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
12835         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
12836         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
12837         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
12838         final double[] bias1 = calibrator.getBias();
12839         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12840         final double[] bias2 = new double[3];
12841         calibrator.getBias(bias2);
12842         assertArrayEquals(bias1, bias2, 0.0);
12843         final Matrix b1 = calibrator.getBiasAsMatrix();
12844         assertEquals(b1, ba);
12845         final Matrix b2 = new Matrix(3, 1);
12846         calibrator.getBiasAsMatrix(b2);
12847         assertEquals(b1, b2);
12848         final Matrix ma1 = new Matrix(3, 3);
12849         ma1.setSubmatrix(0, 0,
12850                 2, 2,
12851                 new double[]{sx, myx, mzx,
12852                         mxy, sy, mzy,
12853                         mxz, myz, sz});
12854         assertEquals(calibrator.getInitialMa(), ma1);
12855         final Matrix ma2 = new Matrix(3, 3);
12856         calibrator.getInitialMa(ma2);
12857         assertEquals(ma1, ma2);
12858         assertNull(calibrator.getMeasurements());
12859         assertFalse(calibrator.isCommonAxisUsed());
12860         assertNull(calibrator.getListener());
12861         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12862         assertFalse(calibrator.isReady());
12863         assertFalse(calibrator.isRunning());
12864         assertNull(calibrator.getEstimatedMa());
12865         assertNull(calibrator.getEstimatedSx());
12866         assertNull(calibrator.getEstimatedSy());
12867         assertNull(calibrator.getEstimatedSz());
12868         assertNull(calibrator.getEstimatedMxy());
12869         assertNull(calibrator.getEstimatedMxz());
12870         assertNull(calibrator.getEstimatedMyx());
12871         assertNull(calibrator.getEstimatedMyz());
12872         assertNull(calibrator.getEstimatedMzx());
12873         assertNull(calibrator.getEstimatedMzy());
12874         assertNull(calibrator.getEstimatedCovariance());
12875         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12876         assertNotNull(calibrator.getGroundTruthGravityNorm());
12877         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12878         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12879         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12880                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12881         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12882         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12883         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12884 
12885         // Force IllegalArgumentException
12886         calibrator = null;
12887         try {
12888             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
12889                     -gravityNorm, biasX, biasY, biasZ, sx, sy, sz,
12890                     mxy, mxz, myx, myz, mzx, mzy);
12891             fail("IllegalArgumentException expected but not thrown");
12892         } catch (final IllegalArgumentException ignore) {
12893         }
12894         assertNull(calibrator);
12895     }
12896 
12897     @Test
12898     public void testConstructor119() throws WrongSizeException {
12899         final Collection<StandardDeviationBodyKinematics> measurements =
12900                 Collections.emptyList();
12901 
12902         final Matrix ba = generateBa();
12903         final double biasX = ba.getElementAtIndex(0);
12904         final double biasY = ba.getElementAtIndex(1);
12905         final double biasZ = ba.getElementAtIndex(2);
12906 
12907         final Matrix ma = generateMaCommonAxis();
12908         final double sx = ma.getElementAt(0, 0);
12909         final double sy = ma.getElementAt(1, 1);
12910         final double sz = ma.getElementAt(2, 2);
12911         final double mxy = ma.getElementAt(0, 1);
12912         final double mxz = ma.getElementAt(0, 2);
12913         final double myx = ma.getElementAt(1, 0);
12914         final double myz = ma.getElementAt(1, 2);
12915         final double mzx = ma.getElementAt(2, 0);
12916         final double mzy = ma.getElementAt(2, 1);
12917 
12918         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12919         final double latitude = Math.toRadians(
12920                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12921         final double longitude = Math.toRadians(
12922                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12923         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12924         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12925         final NEDVelocity nedVelocity = new NEDVelocity();
12926         final ECEFPosition ecefPosition = new ECEFPosition();
12927         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12928         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12929                 ecefPosition, ecefVelocity);
12930         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12931                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12932         final double gravityNorm = gravity.getNorm();
12933 
12934         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
12935                 new KnownBiasAndGravityNormAccelerometerCalibrator(
12936                         gravityNorm, measurements,
12937                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
12938                         myx, myz, mzx, mzy);
12939 
12940         // check default values
12941         assertEquals(calibrator.getBiasX(), biasX, 0.0);
12942         assertEquals(calibrator.getBiasY(), biasY, 0.0);
12943         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
12944         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
12945         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12946         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12947         final Acceleration bx2 = new Acceleration(0.0,
12948                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12949         calibrator.getBiasXAsAcceleration(bx2);
12950         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12951         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12952         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
12953         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12954         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12955         final Acceleration by2 = new Acceleration(0.0,
12956                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12957         calibrator.getBiasYAsAcceleration(by2);
12958         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12959         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12960         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
12961         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12962         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12963         final Acceleration bz2 = new Acceleration(0.0,
12964                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12965         calibrator.getBiasZAsAcceleration(bz2);
12966         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12967         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12968         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12969         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12970         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12971         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
12972         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
12973         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
12974         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
12975         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
12976         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
12977         final double[] bias1 = calibrator.getBias();
12978         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12979         final double[] bias2 = new double[3];
12980         calibrator.getBias(bias2);
12981         assertArrayEquals(bias1, bias2, 0.0);
12982         final Matrix b1 = calibrator.getBiasAsMatrix();
12983         assertEquals(b1, ba);
12984         final Matrix b2 = new Matrix(3, 1);
12985         calibrator.getBiasAsMatrix(b2);
12986         assertEquals(b1, b2);
12987         final Matrix ma1 = new Matrix(3, 3);
12988         ma1.setSubmatrix(0, 0,
12989                 2, 2,
12990                 new double[]{sx, myx, mzx,
12991                         mxy, sy, mzy,
12992                         mxz, myz, sz});
12993         assertEquals(calibrator.getInitialMa(), ma1);
12994         final Matrix ma2 = new Matrix(3, 3);
12995         calibrator.getInitialMa(ma2);
12996         assertEquals(ma1, ma2);
12997         assertSame(calibrator.getMeasurements(), measurements);
12998         assertFalse(calibrator.isCommonAxisUsed());
12999         assertNull(calibrator.getListener());
13000         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13001         assertFalse(calibrator.isReady());
13002         assertFalse(calibrator.isRunning());
13003         assertNull(calibrator.getEstimatedMa());
13004         assertNull(calibrator.getEstimatedSx());
13005         assertNull(calibrator.getEstimatedSy());
13006         assertNull(calibrator.getEstimatedSz());
13007         assertNull(calibrator.getEstimatedMxy());
13008         assertNull(calibrator.getEstimatedMxz());
13009         assertNull(calibrator.getEstimatedMyx());
13010         assertNull(calibrator.getEstimatedMyz());
13011         assertNull(calibrator.getEstimatedMzx());
13012         assertNull(calibrator.getEstimatedMzy());
13013         assertNull(calibrator.getEstimatedCovariance());
13014         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13015         assertNotNull(calibrator.getGroundTruthGravityNorm());
13016         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13017         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13018         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13019                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13020         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13021         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13022         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13023 
13024         // Force IllegalArgumentException
13025         calibrator = null;
13026         try {
13027             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
13028                     -gravityNorm, measurements,
13029                     biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
13030                     myx, myz, mzx, mzy);
13031             fail("IllegalArgumentException expected but not thrown");
13032         } catch (final IllegalArgumentException ignore) {
13033         }
13034         assertNull(calibrator);
13035     }
13036 
13037     @Test
13038     public void testConstructor120() throws WrongSizeException {
13039         final Collection<StandardDeviationBodyKinematics> measurements =
13040                 Collections.emptyList();
13041 
13042         final Matrix ba = generateBa();
13043         final double biasX = ba.getElementAtIndex(0);
13044         final double biasY = ba.getElementAtIndex(1);
13045         final double biasZ = ba.getElementAtIndex(2);
13046 
13047         final Matrix ma = generateMaCommonAxis();
13048         final double sx = ma.getElementAt(0, 0);
13049         final double sy = ma.getElementAt(1, 1);
13050         final double sz = ma.getElementAt(2, 2);
13051         final double mxy = ma.getElementAt(0, 1);
13052         final double mxz = ma.getElementAt(0, 2);
13053         final double myx = ma.getElementAt(1, 0);
13054         final double myz = ma.getElementAt(1, 2);
13055         final double mzx = ma.getElementAt(2, 0);
13056         final double mzy = ma.getElementAt(2, 1);
13057 
13058         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13059         final double latitude = Math.toRadians(
13060                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13061         final double longitude = Math.toRadians(
13062                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13063         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13064         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13065         final NEDVelocity nedVelocity = new NEDVelocity();
13066         final ECEFPosition ecefPosition = new ECEFPosition();
13067         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13068         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13069                 ecefPosition, ecefVelocity);
13070         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13071                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13072         final double gravityNorm = gravity.getNorm();
13073 
13074         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
13075                 new KnownBiasAndGravityNormAccelerometerCalibrator(
13076                         gravityNorm, measurements,
13077                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
13078                         myx, myz, mzx, mzy, this);
13079 
13080         // check default values
13081         assertEquals(calibrator.getBiasX(), biasX, 0.0);
13082         assertEquals(calibrator.getBiasY(), biasY, 0.0);
13083         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
13084         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
13085         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13086         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13087         final Acceleration bx2 = new Acceleration(0.0,
13088                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13089         calibrator.getBiasXAsAcceleration(bx2);
13090         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13091         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13092         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
13093         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13094         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13095         final Acceleration by2 = new Acceleration(0.0,
13096                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13097         calibrator.getBiasYAsAcceleration(by2);
13098         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13099         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13100         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
13101         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13102         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13103         final Acceleration bz2 = new Acceleration(0.0,
13104                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13105         calibrator.getBiasZAsAcceleration(bz2);
13106         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13107         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13108         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13109         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13110         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13111         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13112         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13113         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13114         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13115         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13116         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13117         final double[] bias1 = calibrator.getBias();
13118         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13119         final double[] bias2 = new double[3];
13120         calibrator.getBias(bias2);
13121         assertArrayEquals(bias1, bias2, 0.0);
13122         final Matrix b1 = calibrator.getBiasAsMatrix();
13123         assertEquals(b1, ba);
13124         final Matrix b2 = new Matrix(3, 1);
13125         calibrator.getBiasAsMatrix(b2);
13126         assertEquals(b1, b2);
13127         final Matrix ma1 = new Matrix(3, 3);
13128         ma1.setSubmatrix(0, 0,
13129                 2, 2,
13130                 new double[]{sx, myx, mzx,
13131                         mxy, sy, mzy,
13132                         mxz, myz, sz});
13133         assertEquals(calibrator.getInitialMa(), ma1);
13134         final Matrix ma2 = new Matrix(3, 3);
13135         calibrator.getInitialMa(ma2);
13136         assertEquals(ma1, ma2);
13137         assertSame(calibrator.getMeasurements(), measurements);
13138         assertFalse(calibrator.isCommonAxisUsed());
13139         assertSame(calibrator.getListener(), this);
13140         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13141         assertFalse(calibrator.isReady());
13142         assertFalse(calibrator.isRunning());
13143         assertNull(calibrator.getEstimatedMa());
13144         assertNull(calibrator.getEstimatedSx());
13145         assertNull(calibrator.getEstimatedSy());
13146         assertNull(calibrator.getEstimatedSz());
13147         assertNull(calibrator.getEstimatedMxy());
13148         assertNull(calibrator.getEstimatedMxz());
13149         assertNull(calibrator.getEstimatedMyx());
13150         assertNull(calibrator.getEstimatedMyz());
13151         assertNull(calibrator.getEstimatedMzx());
13152         assertNull(calibrator.getEstimatedMzy());
13153         assertNull(calibrator.getEstimatedCovariance());
13154         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13155         assertNotNull(calibrator.getGroundTruthGravityNorm());
13156         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13157         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13158         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13159                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13160         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13161         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13162         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13163 
13164         // Force IllegalArgumentException
13165         calibrator = null;
13166         try {
13167             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
13168                     -gravityNorm, measurements,
13169                     biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
13170                     myx, myz, mzx, mzy, this);
13171             fail("IllegalArgumentException expected but not thrown");
13172         } catch (final IllegalArgumentException ignore) {
13173         }
13174         assertNull(calibrator);
13175     }
13176 
13177     @Test
13178     public void testConstructor121() throws WrongSizeException {
13179         final Matrix ba = generateBa();
13180         final double biasX = ba.getElementAtIndex(0);
13181         final double biasY = ba.getElementAtIndex(1);
13182         final double biasZ = ba.getElementAtIndex(2);
13183 
13184         final Matrix ma = generateMaCommonAxis();
13185         final double sx = ma.getElementAt(0, 0);
13186         final double sy = ma.getElementAt(1, 1);
13187         final double sz = ma.getElementAt(2, 2);
13188         final double mxy = ma.getElementAt(0, 1);
13189         final double mxz = ma.getElementAt(0, 2);
13190         final double myx = ma.getElementAt(1, 0);
13191         final double myz = ma.getElementAt(1, 2);
13192         final double mzx = ma.getElementAt(2, 0);
13193         final double mzy = ma.getElementAt(2, 1);
13194 
13195         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13196         final double latitude = Math.toRadians(
13197                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13198         final double longitude = Math.toRadians(
13199                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13200         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13201         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13202         final NEDVelocity nedVelocity = new NEDVelocity();
13203         final ECEFPosition ecefPosition = new ECEFPosition();
13204         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13205         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13206                 ecefPosition, ecefVelocity);
13207         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13208                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13209         final double gravityNorm = gravity.getNorm();
13210 
13211         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
13212                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
13213                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
13214                         myx, myz, mzx, mzy);
13215 
13216         // check default values
13217         assertEquals(calibrator.getBiasX(), biasX, 0.0);
13218         assertEquals(calibrator.getBiasY(), biasY, 0.0);
13219         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
13220         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
13221         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13222         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13223         final Acceleration bx2 = new Acceleration(0.0,
13224                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13225         calibrator.getBiasXAsAcceleration(bx2);
13226         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13227         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13228         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
13229         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13230         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13231         final Acceleration by2 = new Acceleration(0.0,
13232                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13233         calibrator.getBiasYAsAcceleration(by2);
13234         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13235         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13236         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
13237         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13238         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13239         final Acceleration bz2 = new Acceleration(0.0,
13240                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13241         calibrator.getBiasZAsAcceleration(bz2);
13242         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13243         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13244         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13245         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13246         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13247         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13248         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13249         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13250         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13251         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13252         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13253         final double[] bias1 = calibrator.getBias();
13254         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13255         final double[] bias2 = new double[3];
13256         calibrator.getBias(bias2);
13257         assertArrayEquals(bias1, bias2, 0.0);
13258         final Matrix b1 = calibrator.getBiasAsMatrix();
13259         assertEquals(b1, ba);
13260         final Matrix b2 = new Matrix(3, 1);
13261         calibrator.getBiasAsMatrix(b2);
13262         assertEquals(b1, b2);
13263         final Matrix ma1 = new Matrix(3, 3);
13264         ma1.setSubmatrix(0, 0,
13265                 2, 2,
13266                 new double[]{sx, myx, mzx,
13267                         mxy, sy, mzy,
13268                         mxz, myz, sz});
13269         assertEquals(calibrator.getInitialMa(), ma1);
13270         final Matrix ma2 = new Matrix(3, 3);
13271         calibrator.getInitialMa(ma2);
13272         assertEquals(ma1, ma2);
13273         assertNull(calibrator.getMeasurements());
13274         assertTrue(calibrator.isCommonAxisUsed());
13275         assertNull(calibrator.getListener());
13276         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
13277         assertFalse(calibrator.isReady());
13278         assertFalse(calibrator.isRunning());
13279         assertNull(calibrator.getEstimatedMa());
13280         assertNull(calibrator.getEstimatedSx());
13281         assertNull(calibrator.getEstimatedSy());
13282         assertNull(calibrator.getEstimatedSz());
13283         assertNull(calibrator.getEstimatedMxy());
13284         assertNull(calibrator.getEstimatedMxz());
13285         assertNull(calibrator.getEstimatedMyx());
13286         assertNull(calibrator.getEstimatedMyz());
13287         assertNull(calibrator.getEstimatedMzx());
13288         assertNull(calibrator.getEstimatedMzy());
13289         assertNull(calibrator.getEstimatedCovariance());
13290         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13291         assertNotNull(calibrator.getGroundTruthGravityNorm());
13292         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13293         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13294         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13295                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13296         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13297         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13298         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13299 
13300         // Force IllegalArgumentException
13301         calibrator = null;
13302         try {
13303             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
13304                     -gravityNorm, true, biasX, biasY, biasZ,
13305                     sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
13306             fail("IllegalArgumentException expected but not thrown");
13307         } catch (final IllegalArgumentException ignore) {
13308         }
13309         assertNull(calibrator);
13310     }
13311 
13312     @Test
13313     public void testConstructor122() throws WrongSizeException {
13314         final Matrix ba = generateBa();
13315         final double biasX = ba.getElementAtIndex(0);
13316         final double biasY = ba.getElementAtIndex(1);
13317         final double biasZ = ba.getElementAtIndex(2);
13318 
13319         final Matrix ma = generateMaCommonAxis();
13320         final double sx = ma.getElementAt(0, 0);
13321         final double sy = ma.getElementAt(1, 1);
13322         final double sz = ma.getElementAt(2, 2);
13323         final double mxy = ma.getElementAt(0, 1);
13324         final double mxz = ma.getElementAt(0, 2);
13325         final double myx = ma.getElementAt(1, 0);
13326         final double myz = ma.getElementAt(1, 2);
13327         final double mzx = ma.getElementAt(2, 0);
13328         final double mzy = ma.getElementAt(2, 1);
13329 
13330         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13331         final double latitude = Math.toRadians(
13332                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13333         final double longitude = Math.toRadians(
13334                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13335         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13336         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13337         final NEDVelocity nedVelocity = new NEDVelocity();
13338         final ECEFPosition ecefPosition = new ECEFPosition();
13339         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13340         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13341                 ecefPosition, ecefVelocity);
13342         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13343                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13344         final double gravityNorm = gravity.getNorm();
13345 
13346         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
13347                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
13348                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
13349                         myx, myz, mzx, mzy, this);
13350 
13351         // check default values
13352         assertEquals(calibrator.getBiasX(), biasX, 0.0);
13353         assertEquals(calibrator.getBiasY(), biasY, 0.0);
13354         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
13355         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
13356         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13357         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13358         final Acceleration bx2 = new Acceleration(0.0,
13359                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13360         calibrator.getBiasXAsAcceleration(bx2);
13361         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13362         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13363         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
13364         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13365         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13366         final Acceleration by2 = new Acceleration(0.0,
13367                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13368         calibrator.getBiasYAsAcceleration(by2);
13369         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13370         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13371         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
13372         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13373         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13374         final Acceleration bz2 = new Acceleration(0.0,
13375                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13376         calibrator.getBiasZAsAcceleration(bz2);
13377         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13378         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13379         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13380         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13381         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13382         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13383         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13384         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13385         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13386         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13387         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13388         final double[] bias1 = calibrator.getBias();
13389         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13390         final double[] bias2 = new double[3];
13391         calibrator.getBias(bias2);
13392         assertArrayEquals(bias1, bias2, 0.0);
13393         final Matrix b1 = calibrator.getBiasAsMatrix();
13394         assertEquals(b1, ba);
13395         final Matrix b2 = new Matrix(3, 1);
13396         calibrator.getBiasAsMatrix(b2);
13397         assertEquals(b1, b2);
13398         final Matrix ma1 = new Matrix(3, 3);
13399         ma1.setSubmatrix(0, 0,
13400                 2, 2,
13401                 new double[]{sx, myx, mzx,
13402                         mxy, sy, mzy,
13403                         mxz, myz, sz});
13404         assertEquals(calibrator.getInitialMa(), ma1);
13405         final Matrix ma2 = new Matrix(3, 3);
13406         calibrator.getInitialMa(ma2);
13407         assertEquals(ma1, ma2);
13408         assertNull(calibrator.getMeasurements());
13409         assertTrue(calibrator.isCommonAxisUsed());
13410         assertSame(calibrator.getListener(), this);
13411         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
13412         assertFalse(calibrator.isReady());
13413         assertFalse(calibrator.isRunning());
13414         assertNull(calibrator.getEstimatedMa());
13415         assertNull(calibrator.getEstimatedSx());
13416         assertNull(calibrator.getEstimatedSy());
13417         assertNull(calibrator.getEstimatedSz());
13418         assertNull(calibrator.getEstimatedMxy());
13419         assertNull(calibrator.getEstimatedMxz());
13420         assertNull(calibrator.getEstimatedMyx());
13421         assertNull(calibrator.getEstimatedMyz());
13422         assertNull(calibrator.getEstimatedMzx());
13423         assertNull(calibrator.getEstimatedMzy());
13424         assertNull(calibrator.getEstimatedCovariance());
13425         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13426         assertNotNull(calibrator.getGroundTruthGravityNorm());
13427         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13428         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13429         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13430                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13431         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13432         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13433         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13434 
13435         // Force IllegalArgumentException
13436         calibrator = null;
13437         try {
13438             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
13439                     -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz,
13440                     mxy, mxz, myx, myz, mzx, mzy, this);
13441             fail("IllegalArgumentException expected but not thrown");
13442         } catch (final IllegalArgumentException ignore) {
13443         }
13444         assertNull(calibrator);
13445     }
13446 
13447     @Test
13448     public void testConstructor123() throws WrongSizeException {
13449         final Collection<StandardDeviationBodyKinematics> measurements =
13450                 Collections.emptyList();
13451 
13452         final Matrix ba = generateBa();
13453         final double biasX = ba.getElementAtIndex(0);
13454         final double biasY = ba.getElementAtIndex(1);
13455         final double biasZ = ba.getElementAtIndex(2);
13456 
13457         final Matrix ma = generateMaCommonAxis();
13458         final double sx = ma.getElementAt(0, 0);
13459         final double sy = ma.getElementAt(1, 1);
13460         final double sz = ma.getElementAt(2, 2);
13461         final double mxy = ma.getElementAt(0, 1);
13462         final double mxz = ma.getElementAt(0, 2);
13463         final double myx = ma.getElementAt(1, 0);
13464         final double myz = ma.getElementAt(1, 2);
13465         final double mzx = ma.getElementAt(2, 0);
13466         final double mzy = ma.getElementAt(2, 1);
13467 
13468         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13469         final double latitude = Math.toRadians(
13470                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13471         final double longitude = Math.toRadians(
13472                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13473         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13474         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13475         final NEDVelocity nedVelocity = new NEDVelocity();
13476         final ECEFPosition ecefPosition = new ECEFPosition();
13477         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13478         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13479                 ecefPosition, ecefVelocity);
13480         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13481                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13482         final double gravityNorm = gravity.getNorm();
13483 
13484         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
13485                 new KnownBiasAndGravityNormAccelerometerCalibrator(
13486                         gravityNorm, measurements,
13487                         true, biasX, biasY, biasZ,
13488                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
13489 
13490         // check default values
13491         assertEquals(calibrator.getBiasX(), biasX, 0.0);
13492         assertEquals(calibrator.getBiasY(), biasY, 0.0);
13493         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
13494         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
13495         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13496         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13497         final Acceleration bx2 = new Acceleration(0.0,
13498                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13499         calibrator.getBiasXAsAcceleration(bx2);
13500         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13501         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13502         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
13503         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13504         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13505         final Acceleration by2 = new Acceleration(0.0,
13506                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13507         calibrator.getBiasYAsAcceleration(by2);
13508         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13509         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13510         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
13511         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13512         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13513         final Acceleration bz2 = new Acceleration(0.0,
13514                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13515         calibrator.getBiasZAsAcceleration(bz2);
13516         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13517         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13518         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13519         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13520         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13521         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13522         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13523         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13524         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13525         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13526         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13527         final double[] bias1 = calibrator.getBias();
13528         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13529         final double[] bias2 = new double[3];
13530         calibrator.getBias(bias2);
13531         assertArrayEquals(bias1, bias2, 0.0);
13532         final Matrix b1 = calibrator.getBiasAsMatrix();
13533         assertEquals(b1, ba);
13534         final Matrix b2 = new Matrix(3, 1);
13535         calibrator.getBiasAsMatrix(b2);
13536         assertEquals(b1, b2);
13537         final Matrix ma1 = new Matrix(3, 3);
13538         ma1.setSubmatrix(0, 0,
13539                 2, 2,
13540                 new double[]{sx, myx, mzx,
13541                         mxy, sy, mzy,
13542                         mxz, myz, sz});
13543         assertEquals(calibrator.getInitialMa(), ma1);
13544         final Matrix ma2 = new Matrix(3, 3);
13545         calibrator.getInitialMa(ma2);
13546         assertEquals(ma1, ma2);
13547         assertSame(calibrator.getMeasurements(), measurements);
13548         assertTrue(calibrator.isCommonAxisUsed());
13549         assertNull(calibrator.getListener());
13550         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
13551         assertFalse(calibrator.isReady());
13552         assertFalse(calibrator.isRunning());
13553         assertNull(calibrator.getEstimatedMa());
13554         assertNull(calibrator.getEstimatedSx());
13555         assertNull(calibrator.getEstimatedSy());
13556         assertNull(calibrator.getEstimatedSz());
13557         assertNull(calibrator.getEstimatedMxy());
13558         assertNull(calibrator.getEstimatedMxz());
13559         assertNull(calibrator.getEstimatedMyx());
13560         assertNull(calibrator.getEstimatedMyz());
13561         assertNull(calibrator.getEstimatedMzx());
13562         assertNull(calibrator.getEstimatedMzy());
13563         assertNull(calibrator.getEstimatedCovariance());
13564         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13565         assertNotNull(calibrator.getGroundTruthGravityNorm());
13566         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13567         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13568         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13569                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13570         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13571         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13572         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13573 
13574         // Force IllegalArgumentException
13575         calibrator = null;
13576         try {
13577             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
13578                     -gravityNorm, measurements,
13579                     true, biasX, biasY, biasZ,
13580                     sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
13581             fail("IllegalArgumentException expected but not thrown");
13582         } catch (final IllegalArgumentException ignore) {
13583         }
13584         assertNull(calibrator);
13585     }
13586 
13587     @Test
13588     public void testConstructor124() throws WrongSizeException {
13589         final Collection<StandardDeviationBodyKinematics> measurements =
13590                 Collections.emptyList();
13591 
13592         final Matrix ba = generateBa();
13593         final double biasX = ba.getElementAtIndex(0);
13594         final double biasY = ba.getElementAtIndex(1);
13595         final double biasZ = ba.getElementAtIndex(2);
13596 
13597         final Matrix ma = generateMaCommonAxis();
13598         final double sx = ma.getElementAt(0, 0);
13599         final double sy = ma.getElementAt(1, 1);
13600         final double sz = ma.getElementAt(2, 2);
13601         final double mxy = ma.getElementAt(0, 1);
13602         final double mxz = ma.getElementAt(0, 2);
13603         final double myx = ma.getElementAt(1, 0);
13604         final double myz = ma.getElementAt(1, 2);
13605         final double mzx = ma.getElementAt(2, 0);
13606         final double mzy = ma.getElementAt(2, 1);
13607 
13608         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13609         final double latitude = Math.toRadians(
13610                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13611         final double longitude = Math.toRadians(
13612                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13613         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13614         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13615         final NEDVelocity nedVelocity = new NEDVelocity();
13616         final ECEFPosition ecefPosition = new ECEFPosition();
13617         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13618         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13619                 ecefPosition, ecefVelocity);
13620         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13621                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13622         final double gravityNorm = gravity.getNorm();
13623 
13624         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
13625                 new KnownBiasAndGravityNormAccelerometerCalibrator(
13626                         gravityNorm, measurements,
13627                         true, biasX, biasY, biasZ,
13628                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
13629                         this);
13630 
13631         // check default values
13632         assertEquals(calibrator.getBiasX(), biasX, 0.0);
13633         assertEquals(calibrator.getBiasY(), biasY, 0.0);
13634         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
13635         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
13636         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13637         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13638         final Acceleration bx2 = new Acceleration(0.0,
13639                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13640         calibrator.getBiasXAsAcceleration(bx2);
13641         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13642         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13643         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
13644         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13645         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13646         final Acceleration by2 = new Acceleration(0.0,
13647                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13648         calibrator.getBiasYAsAcceleration(by2);
13649         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13650         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13651         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
13652         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13653         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13654         final Acceleration bz2 = new Acceleration(0.0,
13655                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13656         calibrator.getBiasZAsAcceleration(bz2);
13657         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13658         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13659         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13660         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13661         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13662         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13663         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13664         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13665         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13666         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13667         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13668         final double[] bias1 = calibrator.getBias();
13669         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13670         final double[] bias2 = new double[3];
13671         calibrator.getBias(bias2);
13672         assertArrayEquals(bias1, bias2, 0.0);
13673         final Matrix b1 = calibrator.getBiasAsMatrix();
13674         assertEquals(b1, ba);
13675         final Matrix b2 = new Matrix(3, 1);
13676         calibrator.getBiasAsMatrix(b2);
13677         assertEquals(b1, b2);
13678         final Matrix ma1 = new Matrix(3, 3);
13679         ma1.setSubmatrix(0, 0,
13680                 2, 2,
13681                 new double[]{sx, myx, mzx,
13682                         mxy, sy, mzy,
13683                         mxz, myz, sz});
13684         assertEquals(calibrator.getInitialMa(), ma1);
13685         final Matrix ma2 = new Matrix(3, 3);
13686         calibrator.getInitialMa(ma2);
13687         assertEquals(ma1, ma2);
13688         assertSame(calibrator.getMeasurements(), measurements);
13689         assertTrue(calibrator.isCommonAxisUsed());
13690         assertSame(calibrator.getListener(), this);
13691         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
13692         assertFalse(calibrator.isReady());
13693         assertFalse(calibrator.isRunning());
13694         assertNull(calibrator.getEstimatedMa());
13695         assertNull(calibrator.getEstimatedSx());
13696         assertNull(calibrator.getEstimatedSy());
13697         assertNull(calibrator.getEstimatedSz());
13698         assertNull(calibrator.getEstimatedMxy());
13699         assertNull(calibrator.getEstimatedMxz());
13700         assertNull(calibrator.getEstimatedMyx());
13701         assertNull(calibrator.getEstimatedMyz());
13702         assertNull(calibrator.getEstimatedMzx());
13703         assertNull(calibrator.getEstimatedMzy());
13704         assertNull(calibrator.getEstimatedCovariance());
13705         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13706         assertNotNull(calibrator.getGroundTruthGravityNorm());
13707         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13708         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13709         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13710                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13711         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13712         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13713         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13714 
13715         // Force IllegalArgumentException
13716         calibrator = null;
13717         try {
13718             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
13719                     -gravityNorm, measurements,
13720                     true, biasX, biasY, biasZ,
13721                     sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
13722                     this);
13723             fail("IllegalArgumentException expected but not thrown");
13724         } catch (final IllegalArgumentException ignore) {
13725         }
13726         assertNull(calibrator);
13727     }
13728 
13729     @Test
13730     public void testConstructor125() throws WrongSizeException {
13731         final Matrix ba = generateBa();
13732         final double biasX = ba.getElementAtIndex(0);
13733         final double biasY = ba.getElementAtIndex(1);
13734         final double biasZ = ba.getElementAtIndex(2);
13735 
13736         final Acceleration bx = new Acceleration(biasX,
13737                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13738         final Acceleration by = new Acceleration(biasY,
13739                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13740         final Acceleration bz = new Acceleration(biasZ,
13741                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13742 
13743         final Matrix ma = generateMaCommonAxis();
13744         final double sx = ma.getElementAt(0, 0);
13745         final double sy = ma.getElementAt(1, 1);
13746         final double sz = ma.getElementAt(2, 2);
13747         final double mxy = ma.getElementAt(0, 1);
13748         final double mxz = ma.getElementAt(0, 2);
13749         final double myx = ma.getElementAt(1, 0);
13750         final double myz = ma.getElementAt(1, 2);
13751         final double mzx = ma.getElementAt(2, 0);
13752         final double mzy = ma.getElementAt(2, 1);
13753 
13754         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13755         final double latitude = Math.toRadians(
13756                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13757         final double longitude = Math.toRadians(
13758                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13759         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13760         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13761         final NEDVelocity nedVelocity = new NEDVelocity();
13762         final ECEFPosition ecefPosition = new ECEFPosition();
13763         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13764         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13765                 ecefPosition, ecefVelocity);
13766         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13767                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13768         final double gravityNorm = gravity.getNorm();
13769 
13770         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
13771                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
13772                         bx, by, bz, sx, sy, sz, mxy, mxz,
13773                         myx, myz, mzx, mzy);
13774 
13775         // check default values
13776         assertEquals(calibrator.getBiasX(), biasX, 0.0);
13777         assertEquals(calibrator.getBiasY(), biasY, 0.0);
13778         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
13779         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
13780         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13781         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13782         final Acceleration bx2 = new Acceleration(0.0,
13783                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13784         calibrator.getBiasXAsAcceleration(bx2);
13785         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13786         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13787         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
13788         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13789         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13790         final Acceleration by2 = new Acceleration(0.0,
13791                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13792         calibrator.getBiasYAsAcceleration(by2);
13793         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13794         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13795         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
13796         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13797         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13798         final Acceleration bz2 = new Acceleration(0.0,
13799                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13800         calibrator.getBiasZAsAcceleration(bz2);
13801         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13802         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13803         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13804         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13805         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13806         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13807         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13808         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13809         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13810         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13811         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13812         final double[] bias1 = calibrator.getBias();
13813         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13814         final double[] bias2 = new double[3];
13815         calibrator.getBias(bias2);
13816         assertArrayEquals(bias1, bias2, 0.0);
13817         final Matrix b1 = calibrator.getBiasAsMatrix();
13818         assertEquals(b1, ba);
13819         final Matrix b2 = new Matrix(3, 1);
13820         calibrator.getBiasAsMatrix(b2);
13821         assertEquals(b1, b2);
13822         final Matrix ma1 = new Matrix(3, 3);
13823         ma1.setSubmatrix(0, 0,
13824                 2, 2,
13825                 new double[]{sx, myx, mzx,
13826                         mxy, sy, mzy,
13827                         mxz, myz, sz});
13828         assertEquals(calibrator.getInitialMa(), ma1);
13829         final Matrix ma2 = new Matrix(3, 3);
13830         calibrator.getInitialMa(ma2);
13831         assertEquals(ma1, ma2);
13832         assertNull(calibrator.getMeasurements());
13833         assertFalse(calibrator.isCommonAxisUsed());
13834         assertNull(calibrator.getListener());
13835         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13836         assertFalse(calibrator.isReady());
13837         assertFalse(calibrator.isRunning());
13838         assertNull(calibrator.getEstimatedMa());
13839         assertNull(calibrator.getEstimatedSx());
13840         assertNull(calibrator.getEstimatedSy());
13841         assertNull(calibrator.getEstimatedSz());
13842         assertNull(calibrator.getEstimatedMxy());
13843         assertNull(calibrator.getEstimatedMxz());
13844         assertNull(calibrator.getEstimatedMyx());
13845         assertNull(calibrator.getEstimatedMyz());
13846         assertNull(calibrator.getEstimatedMzx());
13847         assertNull(calibrator.getEstimatedMzy());
13848         assertNull(calibrator.getEstimatedCovariance());
13849         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13850         assertNotNull(calibrator.getGroundTruthGravityNorm());
13851         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13852         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13853         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13854                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13855         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13856         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13857         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13858 
13859         // Force IllegalArgumentException
13860         calibrator = null;
13861         try {
13862             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
13863                     -gravityNorm, bx, by, bz, sx, sy, sz, mxy, mxz,
13864                     myx, myz, mzx, mzy);
13865             fail("IllegalArgumentException expected but not thrown");
13866         } catch (final IllegalArgumentException ignore) {
13867         }
13868         assertNull(calibrator);
13869     }
13870 
13871     @Test
13872     public void testConstructor126() throws WrongSizeException {
13873         final Matrix ba = generateBa();
13874         final double biasX = ba.getElementAtIndex(0);
13875         final double biasY = ba.getElementAtIndex(1);
13876         final double biasZ = ba.getElementAtIndex(2);
13877 
13878         final Acceleration bx = new Acceleration(biasX,
13879                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13880         final Acceleration by = new Acceleration(biasY,
13881                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13882         final Acceleration bz = new Acceleration(biasZ,
13883                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13884 
13885         final Matrix ma = generateMaCommonAxis();
13886         final double sx = ma.getElementAt(0, 0);
13887         final double sy = ma.getElementAt(1, 1);
13888         final double sz = ma.getElementAt(2, 2);
13889         final double mxy = ma.getElementAt(0, 1);
13890         final double mxz = ma.getElementAt(0, 2);
13891         final double myx = ma.getElementAt(1, 0);
13892         final double myz = ma.getElementAt(1, 2);
13893         final double mzx = ma.getElementAt(2, 0);
13894         final double mzy = ma.getElementAt(2, 1);
13895 
13896         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13897         final double latitude = Math.toRadians(
13898                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13899         final double longitude = Math.toRadians(
13900                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13901         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13902         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13903         final NEDVelocity nedVelocity = new NEDVelocity();
13904         final ECEFPosition ecefPosition = new ECEFPosition();
13905         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13906         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13907                 ecefPosition, ecefVelocity);
13908         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13909                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13910         final double gravityNorm = gravity.getNorm();
13911 
13912         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
13913                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
13914                         bx, by, bz, sx, sy, sz, mxy, mxz,
13915                         myx, myz, mzx, mzy, this);
13916 
13917         // check default values
13918         assertEquals(calibrator.getBiasX(), biasX, 0.0);
13919         assertEquals(calibrator.getBiasY(), biasY, 0.0);
13920         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
13921         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
13922         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13923         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13924         final Acceleration bx2 = new Acceleration(0.0,
13925                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13926         calibrator.getBiasXAsAcceleration(bx2);
13927         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13928         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13929         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
13930         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13931         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13932         final Acceleration by2 = new Acceleration(0.0,
13933                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13934         calibrator.getBiasYAsAcceleration(by2);
13935         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13936         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13937         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
13938         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13939         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13940         final Acceleration bz2 = new Acceleration(0.0,
13941                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13942         calibrator.getBiasZAsAcceleration(bz2);
13943         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13944         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13945         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13946         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13947         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13948         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13949         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13950         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13951         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13952         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13953         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13954         final double[] bias1 = calibrator.getBias();
13955         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13956         final double[] bias2 = new double[3];
13957         calibrator.getBias(bias2);
13958         assertArrayEquals(bias1, bias2, 0.0);
13959         final Matrix b1 = calibrator.getBiasAsMatrix();
13960         assertEquals(b1, ba);
13961         final Matrix b2 = new Matrix(3, 1);
13962         calibrator.getBiasAsMatrix(b2);
13963         assertEquals(b1, b2);
13964         final Matrix ma1 = new Matrix(3, 3);
13965         ma1.setSubmatrix(0, 0,
13966                 2, 2,
13967                 new double[]{sx, myx, mzx,
13968                         mxy, sy, mzy,
13969                         mxz, myz, sz});
13970         assertEquals(calibrator.getInitialMa(), ma1);
13971         final Matrix ma2 = new Matrix(3, 3);
13972         calibrator.getInitialMa(ma2);
13973         assertEquals(ma1, ma2);
13974         assertNull(calibrator.getMeasurements());
13975         assertFalse(calibrator.isCommonAxisUsed());
13976         assertSame(calibrator.getListener(), this);
13977         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13978         assertFalse(calibrator.isReady());
13979         assertFalse(calibrator.isRunning());
13980         assertNull(calibrator.getEstimatedMa());
13981         assertNull(calibrator.getEstimatedSx());
13982         assertNull(calibrator.getEstimatedSy());
13983         assertNull(calibrator.getEstimatedSz());
13984         assertNull(calibrator.getEstimatedMxy());
13985         assertNull(calibrator.getEstimatedMxz());
13986         assertNull(calibrator.getEstimatedMyx());
13987         assertNull(calibrator.getEstimatedMyz());
13988         assertNull(calibrator.getEstimatedMzx());
13989         assertNull(calibrator.getEstimatedMzy());
13990         assertNull(calibrator.getEstimatedCovariance());
13991         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13992         assertNotNull(calibrator.getGroundTruthGravityNorm());
13993         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13994         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13995         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13996                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13997         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13998         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13999         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14000 
14001         // Force IllegalArgumentException
14002         calibrator = null;
14003         try {
14004             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
14005                     -gravityNorm, bx, by, bz, sx, sy, sz, mxy, mxz,
14006                     myx, myz, mzx, mzy, this);
14007             fail("IllegalArgumentException expected but not thrown");
14008         } catch (final IllegalArgumentException ignore) {
14009         }
14010         assertNull(calibrator);
14011     }
14012 
14013     @Test
14014     public void testConstructor127() throws WrongSizeException {
14015         final Collection<StandardDeviationBodyKinematics> measurements =
14016                 Collections.emptyList();
14017 
14018         final Matrix ba = generateBa();
14019         final double biasX = ba.getElementAtIndex(0);
14020         final double biasY = ba.getElementAtIndex(1);
14021         final double biasZ = ba.getElementAtIndex(2);
14022 
14023         final Acceleration bx = new Acceleration(biasX,
14024                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14025         final Acceleration by = new Acceleration(biasY,
14026                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14027         final Acceleration bz = new Acceleration(biasZ,
14028                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14029 
14030         final Matrix ma = generateMaCommonAxis();
14031         final double sx = ma.getElementAt(0, 0);
14032         final double sy = ma.getElementAt(1, 1);
14033         final double sz = ma.getElementAt(2, 2);
14034         final double mxy = ma.getElementAt(0, 1);
14035         final double mxz = ma.getElementAt(0, 2);
14036         final double myx = ma.getElementAt(1, 0);
14037         final double myz = ma.getElementAt(1, 2);
14038         final double mzx = ma.getElementAt(2, 0);
14039         final double mzy = ma.getElementAt(2, 1);
14040 
14041         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14042         final double latitude = Math.toRadians(
14043                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14044         final double longitude = Math.toRadians(
14045                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14046         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14047         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14048         final NEDVelocity nedVelocity = new NEDVelocity();
14049         final ECEFPosition ecefPosition = new ECEFPosition();
14050         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14051         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14052                 ecefPosition, ecefVelocity);
14053         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14054                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14055         final double gravityNorm = gravity.getNorm();
14056 
14057         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
14058                 new KnownBiasAndGravityNormAccelerometerCalibrator(
14059                         gravityNorm, measurements,
14060                         bx, by, bz, sx, sy, sz, mxy, mxz,
14061                         myx, myz, mzx, mzy);
14062 
14063         // check default values
14064         assertEquals(calibrator.getBiasX(), biasX, 0.0);
14065         assertEquals(calibrator.getBiasY(), biasY, 0.0);
14066         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
14067         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
14068         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14069         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14070         final Acceleration bx2 = new Acceleration(0.0,
14071                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14072         calibrator.getBiasXAsAcceleration(bx2);
14073         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14074         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14075         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
14076         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14077         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14078         final Acceleration by2 = new Acceleration(0.0,
14079                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14080         calibrator.getBiasYAsAcceleration(by2);
14081         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14082         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14083         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
14084         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14085         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14086         final Acceleration bz2 = new Acceleration(0.0,
14087                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14088         calibrator.getBiasZAsAcceleration(bz2);
14089         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14090         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14091         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14092         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14093         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14094         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14095         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14096         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14097         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14098         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14099         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14100         final double[] bias1 = calibrator.getBias();
14101         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14102         final double[] bias2 = new double[3];
14103         calibrator.getBias(bias2);
14104         assertArrayEquals(bias1, bias2, 0.0);
14105         final Matrix b1 = calibrator.getBiasAsMatrix();
14106         assertEquals(b1, ba);
14107         final Matrix b2 = new Matrix(3, 1);
14108         calibrator.getBiasAsMatrix(b2);
14109         assertEquals(b1, b2);
14110         final Matrix ma1 = new Matrix(3, 3);
14111         ma1.setSubmatrix(0, 0,
14112                 2, 2,
14113                 new double[]{sx, myx, mzx,
14114                         mxy, sy, mzy,
14115                         mxz, myz, sz});
14116         assertEquals(calibrator.getInitialMa(), ma1);
14117         final Matrix ma2 = new Matrix(3, 3);
14118         calibrator.getInitialMa(ma2);
14119         assertEquals(ma1, ma2);
14120         assertSame(calibrator.getMeasurements(), measurements);
14121         assertFalse(calibrator.isCommonAxisUsed());
14122         assertNull(calibrator.getListener());
14123         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14124         assertFalse(calibrator.isReady());
14125         assertFalse(calibrator.isRunning());
14126         assertNull(calibrator.getEstimatedMa());
14127         assertNull(calibrator.getEstimatedSx());
14128         assertNull(calibrator.getEstimatedSy());
14129         assertNull(calibrator.getEstimatedSz());
14130         assertNull(calibrator.getEstimatedMxy());
14131         assertNull(calibrator.getEstimatedMxz());
14132         assertNull(calibrator.getEstimatedMyx());
14133         assertNull(calibrator.getEstimatedMyz());
14134         assertNull(calibrator.getEstimatedMzx());
14135         assertNull(calibrator.getEstimatedMzy());
14136         assertNull(calibrator.getEstimatedCovariance());
14137         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14138         assertNotNull(calibrator.getGroundTruthGravityNorm());
14139         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14140         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14141         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14142                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14143         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14144         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14145         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14146 
14147         // Force IllegalArgumentException
14148         calibrator = null;
14149         try {
14150             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
14151                     -gravityNorm, measurements,
14152                     bx, by, bz, sx, sy, sz, mxy, mxz,
14153                     myx, myz, mzx, mzy);
14154             fail("IllegalArgumentException expected but not thrown");
14155         } catch (final IllegalArgumentException ignore) {
14156         }
14157         assertNull(calibrator);
14158     }
14159 
14160     @Test
14161     public void testConstructor128() throws WrongSizeException {
14162         final Collection<StandardDeviationBodyKinematics> measurements =
14163                 Collections.emptyList();
14164 
14165         final Matrix ba = generateBa();
14166         final double biasX = ba.getElementAtIndex(0);
14167         final double biasY = ba.getElementAtIndex(1);
14168         final double biasZ = ba.getElementAtIndex(2);
14169 
14170         final Acceleration bx = new Acceleration(biasX,
14171                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14172         final Acceleration by = new Acceleration(biasY,
14173                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14174         final Acceleration bz = new Acceleration(biasZ,
14175                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14176 
14177         final Matrix ma = generateMaCommonAxis();
14178         final double sx = ma.getElementAt(0, 0);
14179         final double sy = ma.getElementAt(1, 1);
14180         final double sz = ma.getElementAt(2, 2);
14181         final double mxy = ma.getElementAt(0, 1);
14182         final double mxz = ma.getElementAt(0, 2);
14183         final double myx = ma.getElementAt(1, 0);
14184         final double myz = ma.getElementAt(1, 2);
14185         final double mzx = ma.getElementAt(2, 0);
14186         final double mzy = ma.getElementAt(2, 1);
14187 
14188         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14189         final double latitude = Math.toRadians(
14190                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14191         final double longitude = Math.toRadians(
14192                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14193         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14194         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14195         final NEDVelocity nedVelocity = new NEDVelocity();
14196         final ECEFPosition ecefPosition = new ECEFPosition();
14197         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14198         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14199                 ecefPosition, ecefVelocity);
14200         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14201                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14202         final double gravityNorm = gravity.getNorm();
14203 
14204         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
14205                 new KnownBiasAndGravityNormAccelerometerCalibrator(
14206                         gravityNorm, measurements,
14207                         bx, by, bz, sx, sy, sz, mxy, mxz,
14208                         myx, myz, mzx, mzy, this);
14209 
14210         // check default values
14211         assertEquals(calibrator.getBiasX(), biasX, 0.0);
14212         assertEquals(calibrator.getBiasY(), biasY, 0.0);
14213         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
14214         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
14215         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14216         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14217         final Acceleration bx2 = new Acceleration(0.0,
14218                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14219         calibrator.getBiasXAsAcceleration(bx2);
14220         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14221         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14222         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
14223         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14224         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14225         final Acceleration by2 = new Acceleration(0.0,
14226                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14227         calibrator.getBiasYAsAcceleration(by2);
14228         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14229         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14230         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
14231         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14232         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14233         final Acceleration bz2 = new Acceleration(0.0,
14234                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14235         calibrator.getBiasZAsAcceleration(bz2);
14236         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14237         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14238         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14239         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14240         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14241         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14242         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14243         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14244         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14245         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14246         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14247         final double[] bias1 = calibrator.getBias();
14248         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14249         final double[] bias2 = new double[3];
14250         calibrator.getBias(bias2);
14251         assertArrayEquals(bias1, bias2, 0.0);
14252         final Matrix b1 = calibrator.getBiasAsMatrix();
14253         assertEquals(b1, ba);
14254         final Matrix b2 = new Matrix(3, 1);
14255         calibrator.getBiasAsMatrix(b2);
14256         assertEquals(b1, b2);
14257         final Matrix ma1 = new Matrix(3, 3);
14258         ma1.setSubmatrix(0, 0,
14259                 2, 2,
14260                 new double[]{sx, myx, mzx,
14261                         mxy, sy, mzy,
14262                         mxz, myz, sz});
14263         assertEquals(calibrator.getInitialMa(), ma1);
14264         final Matrix ma2 = new Matrix(3, 3);
14265         calibrator.getInitialMa(ma2);
14266         assertEquals(ma1, ma2);
14267         assertSame(calibrator.getMeasurements(), measurements);
14268         assertFalse(calibrator.isCommonAxisUsed());
14269         assertSame(calibrator.getListener(), this);
14270         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14271         assertFalse(calibrator.isReady());
14272         assertFalse(calibrator.isRunning());
14273         assertNull(calibrator.getEstimatedMa());
14274         assertNull(calibrator.getEstimatedSx());
14275         assertNull(calibrator.getEstimatedSy());
14276         assertNull(calibrator.getEstimatedSz());
14277         assertNull(calibrator.getEstimatedMxy());
14278         assertNull(calibrator.getEstimatedMxz());
14279         assertNull(calibrator.getEstimatedMyx());
14280         assertNull(calibrator.getEstimatedMyz());
14281         assertNull(calibrator.getEstimatedMzx());
14282         assertNull(calibrator.getEstimatedMzy());
14283         assertNull(calibrator.getEstimatedCovariance());
14284         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14285         assertNotNull(calibrator.getGroundTruthGravityNorm());
14286         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14287         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14288         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14289                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14290         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14291         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14292         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14293 
14294         // Force IllegalArgumentException
14295         calibrator = null;
14296         try {
14297             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
14298                     -gravityNorm, measurements,
14299                     bx, by, bz, sx, sy, sz, mxy, mxz,
14300                     myx, myz, mzx, mzy, this);
14301             fail("IllegalArgumentException expected but not thrown");
14302         } catch (final IllegalArgumentException ignore) {
14303         }
14304         assertNull(calibrator);
14305     }
14306 
14307     @Test
14308     public void testConstructor129() throws WrongSizeException {
14309         final Matrix ba = generateBa();
14310         final double biasX = ba.getElementAtIndex(0);
14311         final double biasY = ba.getElementAtIndex(1);
14312         final double biasZ = ba.getElementAtIndex(2);
14313 
14314         final Acceleration bx = new Acceleration(biasX,
14315                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14316         final Acceleration by = new Acceleration(biasY,
14317                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14318         final Acceleration bz = new Acceleration(biasZ,
14319                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14320 
14321         final Matrix ma = generateMaCommonAxis();
14322         final double sx = ma.getElementAt(0, 0);
14323         final double sy = ma.getElementAt(1, 1);
14324         final double sz = ma.getElementAt(2, 2);
14325         final double mxy = ma.getElementAt(0, 1);
14326         final double mxz = ma.getElementAt(0, 2);
14327         final double myx = ma.getElementAt(1, 0);
14328         final double myz = ma.getElementAt(1, 2);
14329         final double mzx = ma.getElementAt(2, 0);
14330         final double mzy = ma.getElementAt(2, 1);
14331 
14332         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14333         final double latitude = Math.toRadians(
14334                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14335         final double longitude = Math.toRadians(
14336                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14337         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14338         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14339         final NEDVelocity nedVelocity = new NEDVelocity();
14340         final ECEFPosition ecefPosition = new ECEFPosition();
14341         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14342         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14343                 ecefPosition, ecefVelocity);
14344         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14345                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14346         final double gravityNorm = gravity.getNorm();
14347 
14348         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
14349                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
14350                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
14351                         myx, myz, mzx, mzy);
14352 
14353         // check default values
14354         assertEquals(calibrator.getBiasX(), biasX, 0.0);
14355         assertEquals(calibrator.getBiasY(), biasY, 0.0);
14356         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
14357         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
14358         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14359         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14360         final Acceleration bx2 = new Acceleration(0.0,
14361                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14362         calibrator.getBiasXAsAcceleration(bx2);
14363         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14364         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14365         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
14366         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14367         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14368         final Acceleration by2 = new Acceleration(0.0,
14369                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14370         calibrator.getBiasYAsAcceleration(by2);
14371         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14372         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14373         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
14374         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14375         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14376         final Acceleration bz2 = new Acceleration(0.0,
14377                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14378         calibrator.getBiasZAsAcceleration(bz2);
14379         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14380         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14381         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14382         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14383         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14384         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14385         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14386         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14387         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14388         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14389         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14390         final double[] bias1 = calibrator.getBias();
14391         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14392         final double[] bias2 = new double[3];
14393         calibrator.getBias(bias2);
14394         assertArrayEquals(bias1, bias2, 0.0);
14395         final Matrix b1 = calibrator.getBiasAsMatrix();
14396         assertEquals(b1, ba);
14397         final Matrix b2 = new Matrix(3, 1);
14398         calibrator.getBiasAsMatrix(b2);
14399         assertEquals(b1, b2);
14400         final Matrix ma1 = new Matrix(3, 3);
14401         ma1.setSubmatrix(0, 0,
14402                 2, 2,
14403                 new double[]{sx, myx, mzx,
14404                         mxy, sy, mzy,
14405                         mxz, myz, sz});
14406         assertEquals(calibrator.getInitialMa(), ma1);
14407         final Matrix ma2 = new Matrix(3, 3);
14408         calibrator.getInitialMa(ma2);
14409         assertEquals(ma1, ma2);
14410         assertNull(calibrator.getMeasurements());
14411         assertTrue(calibrator.isCommonAxisUsed());
14412         assertNull(calibrator.getListener());
14413         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
14414         assertFalse(calibrator.isReady());
14415         assertFalse(calibrator.isRunning());
14416         assertNull(calibrator.getEstimatedMa());
14417         assertNull(calibrator.getEstimatedSx());
14418         assertNull(calibrator.getEstimatedSy());
14419         assertNull(calibrator.getEstimatedSz());
14420         assertNull(calibrator.getEstimatedMxy());
14421         assertNull(calibrator.getEstimatedMxz());
14422         assertNull(calibrator.getEstimatedMyx());
14423         assertNull(calibrator.getEstimatedMyz());
14424         assertNull(calibrator.getEstimatedMzx());
14425         assertNull(calibrator.getEstimatedMzy());
14426         assertNull(calibrator.getEstimatedCovariance());
14427         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14428         assertNotNull(calibrator.getGroundTruthGravityNorm());
14429         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14430         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14431         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14432                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14433         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14434         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14435         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14436 
14437         // Force IllegalArgumentException
14438         calibrator = null;
14439         try {
14440             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
14441                     -gravityNorm, true, bx, by, bz, sx, sy, sz, mxy, mxz,
14442                     myx, myz, mzx, mzy);
14443             fail("IllegalArgumentException expected but not thrown");
14444         } catch (final IllegalArgumentException ignore) {
14445         }
14446         assertNull(calibrator);
14447     }
14448 
14449     @Test
14450     public void testConstructor130() throws WrongSizeException {
14451         final Matrix ba = generateBa();
14452         final double biasX = ba.getElementAtIndex(0);
14453         final double biasY = ba.getElementAtIndex(1);
14454         final double biasZ = ba.getElementAtIndex(2);
14455 
14456         final Acceleration bx = new Acceleration(biasX,
14457                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14458         final Acceleration by = new Acceleration(biasY,
14459                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14460         final Acceleration bz = new Acceleration(biasZ,
14461                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14462 
14463         final Matrix ma = generateMaCommonAxis();
14464         final double sx = ma.getElementAt(0, 0);
14465         final double sy = ma.getElementAt(1, 1);
14466         final double sz = ma.getElementAt(2, 2);
14467         final double mxy = ma.getElementAt(0, 1);
14468         final double mxz = ma.getElementAt(0, 2);
14469         final double myx = ma.getElementAt(1, 0);
14470         final double myz = ma.getElementAt(1, 2);
14471         final double mzx = ma.getElementAt(2, 0);
14472         final double mzy = ma.getElementAt(2, 1);
14473 
14474         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14475         final double latitude = Math.toRadians(
14476                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14477         final double longitude = Math.toRadians(
14478                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14479         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14480         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14481         final NEDVelocity nedVelocity = new NEDVelocity();
14482         final ECEFPosition ecefPosition = new ECEFPosition();
14483         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14484         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14485                 ecefPosition, ecefVelocity);
14486         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14487                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14488         final double gravityNorm = gravity.getNorm();
14489 
14490         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
14491                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
14492                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
14493                         myx, myz, mzx, mzy, this);
14494 
14495         // check default values
14496         assertEquals(calibrator.getBiasX(), biasX, 0.0);
14497         assertEquals(calibrator.getBiasY(), biasY, 0.0);
14498         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
14499         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
14500         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14501         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14502         final Acceleration bx2 = new Acceleration(0.0,
14503                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14504         calibrator.getBiasXAsAcceleration(bx2);
14505         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14506         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14507         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
14508         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14509         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14510         final Acceleration by2 = new Acceleration(0.0,
14511                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14512         calibrator.getBiasYAsAcceleration(by2);
14513         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14514         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14515         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
14516         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14517         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14518         final Acceleration bz2 = new Acceleration(0.0,
14519                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14520         calibrator.getBiasZAsAcceleration(bz2);
14521         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14522         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14523         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14524         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14525         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14526         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14527         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14528         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14529         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14530         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14531         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14532         final double[] bias1 = calibrator.getBias();
14533         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14534         final double[] bias2 = new double[3];
14535         calibrator.getBias(bias2);
14536         assertArrayEquals(bias1, bias2, 0.0);
14537         final Matrix b1 = calibrator.getBiasAsMatrix();
14538         assertEquals(b1, ba);
14539         final Matrix b2 = new Matrix(3, 1);
14540         calibrator.getBiasAsMatrix(b2);
14541         assertEquals(b1, b2);
14542         final Matrix ma1 = new Matrix(3, 3);
14543         ma1.setSubmatrix(0, 0,
14544                 2, 2,
14545                 new double[]{sx, myx, mzx,
14546                         mxy, sy, mzy,
14547                         mxz, myz, sz});
14548         assertEquals(calibrator.getInitialMa(), ma1);
14549         final Matrix ma2 = new Matrix(3, 3);
14550         calibrator.getInitialMa(ma2);
14551         assertEquals(ma1, ma2);
14552         assertNull(calibrator.getMeasurements());
14553         assertTrue(calibrator.isCommonAxisUsed());
14554         assertSame(calibrator.getListener(), this);
14555         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
14556         assertFalse(calibrator.isReady());
14557         assertFalse(calibrator.isRunning());
14558         assertNull(calibrator.getEstimatedMa());
14559         assertNull(calibrator.getEstimatedSx());
14560         assertNull(calibrator.getEstimatedSy());
14561         assertNull(calibrator.getEstimatedSz());
14562         assertNull(calibrator.getEstimatedMxy());
14563         assertNull(calibrator.getEstimatedMxz());
14564         assertNull(calibrator.getEstimatedMyx());
14565         assertNull(calibrator.getEstimatedMyz());
14566         assertNull(calibrator.getEstimatedMzx());
14567         assertNull(calibrator.getEstimatedMzy());
14568         assertNull(calibrator.getEstimatedCovariance());
14569         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14570         assertNotNull(calibrator.getGroundTruthGravityNorm());
14571         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14572         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14573         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14574                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14575         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14576         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14577         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14578 
14579         // Force IllegalArgumentException
14580         calibrator = null;
14581         try {
14582             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
14583                     -gravityNorm, true, bx, by, bz, sx, sy, sz, mxy, mxz,
14584                     myx, myz, mzx, mzy, this);
14585             fail("IllegalArgumentException expected but not thrown");
14586         } catch (final IllegalArgumentException ignore) {
14587         }
14588         assertNull(calibrator);
14589     }
14590 
14591     @Test
14592     public void testConstructor131() throws WrongSizeException {
14593         final Collection<StandardDeviationBodyKinematics> measurements =
14594                 Collections.emptyList();
14595 
14596         final Matrix ba = generateBa();
14597         final double biasX = ba.getElementAtIndex(0);
14598         final double biasY = ba.getElementAtIndex(1);
14599         final double biasZ = ba.getElementAtIndex(2);
14600 
14601         final Acceleration bx = new Acceleration(biasX,
14602                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14603         final Acceleration by = new Acceleration(biasY,
14604                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14605         final Acceleration bz = new Acceleration(biasZ,
14606                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14607 
14608         final Matrix ma = generateMaCommonAxis();
14609         final double sx = ma.getElementAt(0, 0);
14610         final double sy = ma.getElementAt(1, 1);
14611         final double sz = ma.getElementAt(2, 2);
14612         final double mxy = ma.getElementAt(0, 1);
14613         final double mxz = ma.getElementAt(0, 2);
14614         final double myx = ma.getElementAt(1, 0);
14615         final double myz = ma.getElementAt(1, 2);
14616         final double mzx = ma.getElementAt(2, 0);
14617         final double mzy = ma.getElementAt(2, 1);
14618 
14619         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14620         final double latitude = Math.toRadians(
14621                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14622         final double longitude = Math.toRadians(
14623                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14624         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14625         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14626         final NEDVelocity nedVelocity = new NEDVelocity();
14627         final ECEFPosition ecefPosition = new ECEFPosition();
14628         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14629         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14630                 ecefPosition, ecefVelocity);
14631         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14632                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14633         final double gravityNorm = gravity.getNorm();
14634 
14635         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
14636                 new KnownBiasAndGravityNormAccelerometerCalibrator(
14637                         gravityNorm, measurements,
14638                         true, bx, by, bz, sx, sy, sz,
14639                         mxy, mxz, myx, myz, mzx, mzy);
14640 
14641         // check default values
14642         assertEquals(calibrator.getBiasX(), biasX, 0.0);
14643         assertEquals(calibrator.getBiasY(), biasY, 0.0);
14644         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
14645         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
14646         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14647         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14648         final Acceleration bx2 = new Acceleration(0.0,
14649                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14650         calibrator.getBiasXAsAcceleration(bx2);
14651         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14652         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14653         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
14654         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14655         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14656         final Acceleration by2 = new Acceleration(0.0,
14657                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14658         calibrator.getBiasYAsAcceleration(by2);
14659         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14660         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14661         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
14662         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14663         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14664         final Acceleration bz2 = new Acceleration(0.0,
14665                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14666         calibrator.getBiasZAsAcceleration(bz2);
14667         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14668         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14669         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14670         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14671         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14672         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14673         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14674         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14675         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14676         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14677         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14678         final double[] bias1 = calibrator.getBias();
14679         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14680         final double[] bias2 = new double[3];
14681         calibrator.getBias(bias2);
14682         assertArrayEquals(bias1, bias2, 0.0);
14683         final Matrix b1 = calibrator.getBiasAsMatrix();
14684         assertEquals(b1, ba);
14685         final Matrix b2 = new Matrix(3, 1);
14686         calibrator.getBiasAsMatrix(b2);
14687         assertEquals(b1, b2);
14688         final Matrix ma1 = new Matrix(3, 3);
14689         ma1.setSubmatrix(0, 0,
14690                 2, 2,
14691                 new double[]{sx, myx, mzx,
14692                         mxy, sy, mzy,
14693                         mxz, myz, sz});
14694         assertEquals(calibrator.getInitialMa(), ma1);
14695         final Matrix ma2 = new Matrix(3, 3);
14696         calibrator.getInitialMa(ma2);
14697         assertEquals(ma1, ma2);
14698         assertSame(calibrator.getMeasurements(), measurements);
14699         assertTrue(calibrator.isCommonAxisUsed());
14700         assertNull(calibrator.getListener());
14701         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
14702         assertFalse(calibrator.isReady());
14703         assertFalse(calibrator.isRunning());
14704         assertNull(calibrator.getEstimatedMa());
14705         assertNull(calibrator.getEstimatedSx());
14706         assertNull(calibrator.getEstimatedSy());
14707         assertNull(calibrator.getEstimatedSz());
14708         assertNull(calibrator.getEstimatedMxy());
14709         assertNull(calibrator.getEstimatedMxz());
14710         assertNull(calibrator.getEstimatedMyx());
14711         assertNull(calibrator.getEstimatedMyz());
14712         assertNull(calibrator.getEstimatedMzx());
14713         assertNull(calibrator.getEstimatedMzy());
14714         assertNull(calibrator.getEstimatedCovariance());
14715         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14716         assertNotNull(calibrator.getGroundTruthGravityNorm());
14717         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14718         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14719         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14720                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14721         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14722         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14723         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14724 
14725         // Force IllegalArgumentException
14726         calibrator = null;
14727         try {
14728             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
14729                     -gravityNorm, measurements,
14730                     true, bx, by, bz, sx, sy, sz,
14731                     mxy, mxz, myx, myz, mzx, mzy);
14732             fail("IllegalArgumentException expected but not thrown");
14733         } catch (final IllegalArgumentException ignore) {
14734         }
14735         assertNull(calibrator);
14736     }
14737 
14738     @Test
14739     public void testConstructor132() throws WrongSizeException {
14740         final Collection<StandardDeviationBodyKinematics> measurements =
14741                 Collections.emptyList();
14742 
14743         final Matrix ba = generateBa();
14744         final double biasX = ba.getElementAtIndex(0);
14745         final double biasY = ba.getElementAtIndex(1);
14746         final double biasZ = ba.getElementAtIndex(2);
14747 
14748         final Acceleration bx = new Acceleration(biasX,
14749                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14750         final Acceleration by = new Acceleration(biasY,
14751                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14752         final Acceleration bz = new Acceleration(biasZ,
14753                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14754 
14755         final Matrix ma = generateMaCommonAxis();
14756         final double sx = ma.getElementAt(0, 0);
14757         final double sy = ma.getElementAt(1, 1);
14758         final double sz = ma.getElementAt(2, 2);
14759         final double mxy = ma.getElementAt(0, 1);
14760         final double mxz = ma.getElementAt(0, 2);
14761         final double myx = ma.getElementAt(1, 0);
14762         final double myz = ma.getElementAt(1, 2);
14763         final double mzx = ma.getElementAt(2, 0);
14764         final double mzy = ma.getElementAt(2, 1);
14765 
14766         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14767         final double latitude = Math.toRadians(
14768                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14769         final double longitude = Math.toRadians(
14770                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14771         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14772         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14773         final NEDVelocity nedVelocity = new NEDVelocity();
14774         final ECEFPosition ecefPosition = new ECEFPosition();
14775         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14776         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14777                 ecefPosition, ecefVelocity);
14778         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14779                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14780         final double gravityNorm = gravity.getNorm();
14781 
14782         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
14783                 new KnownBiasAndGravityNormAccelerometerCalibrator(
14784                         gravityNorm, measurements,
14785                         true, bx, by, bz, sx, sy, sz,
14786                         mxy, mxz, myx, myz, mzx, mzy, this);
14787 
14788         // check default values
14789         assertEquals(calibrator.getBiasX(), biasX, 0.0);
14790         assertEquals(calibrator.getBiasY(), biasY, 0.0);
14791         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
14792         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
14793         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14794         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14795         final Acceleration bx2 = new Acceleration(0.0,
14796                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14797         calibrator.getBiasXAsAcceleration(bx2);
14798         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14799         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14800         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
14801         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14802         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14803         final Acceleration by2 = new Acceleration(0.0,
14804                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14805         calibrator.getBiasYAsAcceleration(by2);
14806         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14807         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14808         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
14809         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14810         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14811         final Acceleration bz2 = new Acceleration(0.0,
14812                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14813         calibrator.getBiasZAsAcceleration(bz2);
14814         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14815         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14816         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14817         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14818         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14819         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14820         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14821         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14822         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14823         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14824         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14825         final double[] bias1 = calibrator.getBias();
14826         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14827         final double[] bias2 = new double[3];
14828         calibrator.getBias(bias2);
14829         assertArrayEquals(bias1, bias2, 0.0);
14830         final Matrix b1 = calibrator.getBiasAsMatrix();
14831         assertEquals(b1, ba);
14832         final Matrix b2 = new Matrix(3, 1);
14833         calibrator.getBiasAsMatrix(b2);
14834         assertEquals(b1, b2);
14835         final Matrix ma1 = new Matrix(3, 3);
14836         ma1.setSubmatrix(0, 0,
14837                 2, 2,
14838                 new double[]{sx, myx, mzx,
14839                         mxy, sy, mzy,
14840                         mxz, myz, sz});
14841         assertEquals(calibrator.getInitialMa(), ma1);
14842         final Matrix ma2 = new Matrix(3, 3);
14843         calibrator.getInitialMa(ma2);
14844         assertEquals(ma1, ma2);
14845         assertSame(calibrator.getMeasurements(), measurements);
14846         assertTrue(calibrator.isCommonAxisUsed());
14847         assertSame(calibrator.getListener(), this);
14848         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
14849         assertFalse(calibrator.isReady());
14850         assertFalse(calibrator.isRunning());
14851         assertNull(calibrator.getEstimatedMa());
14852         assertNull(calibrator.getEstimatedSx());
14853         assertNull(calibrator.getEstimatedSy());
14854         assertNull(calibrator.getEstimatedSz());
14855         assertNull(calibrator.getEstimatedMxy());
14856         assertNull(calibrator.getEstimatedMxz());
14857         assertNull(calibrator.getEstimatedMyx());
14858         assertNull(calibrator.getEstimatedMyz());
14859         assertNull(calibrator.getEstimatedMzx());
14860         assertNull(calibrator.getEstimatedMzy());
14861         assertNull(calibrator.getEstimatedCovariance());
14862         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14863         assertNotNull(calibrator.getGroundTruthGravityNorm());
14864         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14865         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14866         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14867                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14868         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14869         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14870         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14871 
14872         // Force IllegalArgumentException
14873         calibrator = null;
14874         try {
14875             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
14876                     -gravityNorm, measurements,
14877                     true, bx, by, bz, sx, sy, sz,
14878                     mxy, mxz, myx, myz, mzx, mzy, this);
14879             fail("IllegalArgumentException expected but not thrown");
14880         } catch (final IllegalArgumentException ignore) {
14881         }
14882         assertNull(calibrator);
14883     }
14884 
14885     @Test
14886     public void testConstructor133() throws WrongSizeException {
14887         final Matrix ba = generateBa();
14888         final double[] bias = ba.getBuffer();
14889         final double biasX = ba.getElementAtIndex(0);
14890         final double biasY = ba.getElementAtIndex(1);
14891         final double biasZ = ba.getElementAtIndex(2);
14892 
14893         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14894         final double latitude = Math.toRadians(
14895                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14896         final double longitude = Math.toRadians(
14897                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14898         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14899         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14900         final NEDVelocity nedVelocity = new NEDVelocity();
14901         final ECEFPosition ecefPosition = new ECEFPosition();
14902         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14903         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14904                 ecefPosition, ecefVelocity);
14905         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14906                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14907         final double gravityNorm = gravity.getNorm();
14908 
14909         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
14910                 new KnownBiasAndGravityNormAccelerometerCalibrator(
14911                         gravityNorm, bias);
14912 
14913         // check default values
14914         assertEquals(calibrator.getBiasX(), biasX, 0.0);
14915         assertEquals(calibrator.getBiasY(), biasY, 0.0);
14916         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
14917         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
14918         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14919         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14920         final Acceleration bx2 = new Acceleration(0.0,
14921                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14922         calibrator.getBiasXAsAcceleration(bx2);
14923         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14924         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14925         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
14926         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14927         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14928         final Acceleration by2 = new Acceleration(0.0,
14929                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14930         calibrator.getBiasYAsAcceleration(by2);
14931         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14932         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14933         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
14934         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14935         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14936         final Acceleration bz2 = new Acceleration(0.0,
14937                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14938         calibrator.getBiasZAsAcceleration(bz2);
14939         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14940         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14941         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
14942         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
14943         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
14944         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
14945         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
14946         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
14947         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
14948         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
14949         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
14950         final double[] bias1 = calibrator.getBias();
14951         assertArrayEquals(bias1, bias, 0.0);
14952         final double[] bias2 = new double[3];
14953         calibrator.getBias(bias2);
14954         assertArrayEquals(bias1, bias2, 0.0);
14955         final Matrix b1 = calibrator.getBiasAsMatrix();
14956         assertEquals(b1, ba);
14957         final Matrix b2 = new Matrix(3, 1);
14958         calibrator.getBiasAsMatrix(b2);
14959         assertEquals(b1, b2);
14960         final Matrix ma1 = calibrator.getInitialMa();
14961         assertEquals(ma1, new Matrix(3, 3));
14962         final Matrix ma2 = new Matrix(3, 3);
14963         calibrator.getInitialMa(ma2);
14964         assertEquals(ma1, ma2);
14965         assertNull(calibrator.getMeasurements());
14966         assertFalse(calibrator.isCommonAxisUsed());
14967         assertNull(calibrator.getListener());
14968         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14969         assertFalse(calibrator.isReady());
14970         assertFalse(calibrator.isRunning());
14971         assertNull(calibrator.getEstimatedMa());
14972         assertNull(calibrator.getEstimatedSx());
14973         assertNull(calibrator.getEstimatedSy());
14974         assertNull(calibrator.getEstimatedSz());
14975         assertNull(calibrator.getEstimatedMxy());
14976         assertNull(calibrator.getEstimatedMxz());
14977         assertNull(calibrator.getEstimatedMyx());
14978         assertNull(calibrator.getEstimatedMyz());
14979         assertNull(calibrator.getEstimatedMzx());
14980         assertNull(calibrator.getEstimatedMzy());
14981         assertNull(calibrator.getEstimatedCovariance());
14982         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14983         assertNotNull(calibrator.getGroundTruthGravityNorm());
14984         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14985         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14986         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14987                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14988         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14989         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14990         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14991 
14992         // Force IllegalArgumentException
14993         calibrator = null;
14994         try {
14995             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
14996                     -gravityNorm, bias);
14997             fail("IllegalArgumentException expected but not thrown");
14998         } catch (final IllegalArgumentException ignore) {
14999         }
15000         try {
15001             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
15002                     new double[1]);
15003             fail("IllegalArgumentException expected but not thrown");
15004         } catch (final IllegalArgumentException ignore) {
15005         }
15006         assertNull(calibrator);
15007     }
15008 
15009     @Test
15010     public void testConstructor134() throws WrongSizeException {
15011         final Matrix ba = generateBa();
15012         final double[] bias = ba.getBuffer();
15013         final double biasX = ba.getElementAtIndex(0);
15014         final double biasY = ba.getElementAtIndex(1);
15015         final double biasZ = ba.getElementAtIndex(2);
15016 
15017         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15018         final double latitude = Math.toRadians(
15019                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15020         final double longitude = Math.toRadians(
15021                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15022         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15023         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15024         final NEDVelocity nedVelocity = new NEDVelocity();
15025         final ECEFPosition ecefPosition = new ECEFPosition();
15026         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15027         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15028                 ecefPosition, ecefVelocity);
15029         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15030                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15031         final double gravityNorm = gravity.getNorm();
15032 
15033         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
15034                 new KnownBiasAndGravityNormAccelerometerCalibrator(
15035                         gravityNorm, bias, this);
15036 
15037         // check default values
15038         assertEquals(calibrator.getBiasX(), biasX, 0.0);
15039         assertEquals(calibrator.getBiasY(), biasY, 0.0);
15040         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
15041         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
15042         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15043         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15044         final Acceleration bx2 = new Acceleration(0.0,
15045                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15046         calibrator.getBiasXAsAcceleration(bx2);
15047         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15048         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15049         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
15050         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15051         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15052         final Acceleration by2 = new Acceleration(0.0,
15053                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15054         calibrator.getBiasYAsAcceleration(by2);
15055         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15056         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15057         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
15058         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15059         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15060         final Acceleration bz2 = new Acceleration(0.0,
15061                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15062         calibrator.getBiasZAsAcceleration(bz2);
15063         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15064         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15065         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15066         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15067         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15068         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15069         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15070         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15071         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15072         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15073         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15074         final double[] bias1 = calibrator.getBias();
15075         assertArrayEquals(bias1, bias, 0.0);
15076         final double[] bias2 = new double[3];
15077         calibrator.getBias(bias2);
15078         assertArrayEquals(bias1, bias2, 0.0);
15079         final Matrix b1 = calibrator.getBiasAsMatrix();
15080         assertEquals(b1, ba);
15081         final Matrix b2 = new Matrix(3, 1);
15082         calibrator.getBiasAsMatrix(b2);
15083         assertEquals(b1, b2);
15084         final Matrix ma1 = calibrator.getInitialMa();
15085         assertEquals(ma1, new Matrix(3, 3));
15086         final Matrix ma2 = new Matrix(3, 3);
15087         calibrator.getInitialMa(ma2);
15088         assertEquals(ma1, ma2);
15089         assertNull(calibrator.getMeasurements());
15090         assertFalse(calibrator.isCommonAxisUsed());
15091         assertSame(calibrator.getListener(), this);
15092         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15093         assertFalse(calibrator.isReady());
15094         assertFalse(calibrator.isRunning());
15095         assertNull(calibrator.getEstimatedMa());
15096         assertNull(calibrator.getEstimatedSx());
15097         assertNull(calibrator.getEstimatedSy());
15098         assertNull(calibrator.getEstimatedSz());
15099         assertNull(calibrator.getEstimatedMxy());
15100         assertNull(calibrator.getEstimatedMxz());
15101         assertNull(calibrator.getEstimatedMyx());
15102         assertNull(calibrator.getEstimatedMyz());
15103         assertNull(calibrator.getEstimatedMzx());
15104         assertNull(calibrator.getEstimatedMzy());
15105         assertNull(calibrator.getEstimatedCovariance());
15106         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15107         assertNotNull(calibrator.getGroundTruthGravityNorm());
15108         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15109         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15110         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15111                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15112         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15113         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15114         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15115 
15116         // Force IllegalArgumentException
15117         calibrator = null;
15118         try {
15119             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15120                     -gravityNorm, bias, this);
15121             fail("IllegalArgumentException expected but not thrown");
15122         } catch (final IllegalArgumentException ignore) {
15123         }
15124         try {
15125             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15126                     gravityNorm, new double[1], this);
15127             fail("IllegalArgumentException expected but not thrown");
15128         } catch (final IllegalArgumentException ignore) {
15129         }
15130         assertNull(calibrator);
15131     }
15132 
15133     @Test
15134     public void testConstructor135() throws WrongSizeException {
15135         final Collection<StandardDeviationBodyKinematics> measurements =
15136                 Collections.emptyList();
15137 
15138         final Matrix ba = generateBa();
15139         final double[] bias = ba.getBuffer();
15140         final double biasX = ba.getElementAtIndex(0);
15141         final double biasY = ba.getElementAtIndex(1);
15142         final double biasZ = ba.getElementAtIndex(2);
15143 
15144         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15145         final double latitude = Math.toRadians(
15146                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15147         final double longitude = Math.toRadians(
15148                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15149         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15150         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15151         final NEDVelocity nedVelocity = new NEDVelocity();
15152         final ECEFPosition ecefPosition = new ECEFPosition();
15153         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15154         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15155                 ecefPosition, ecefVelocity);
15156         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15157                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15158         final double gravityNorm = gravity.getNorm();
15159 
15160         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
15161                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
15162                         measurements, bias);
15163 
15164         // check default values
15165         assertEquals(calibrator.getBiasX(), biasX, 0.0);
15166         assertEquals(calibrator.getBiasY(), biasY, 0.0);
15167         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
15168         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
15169         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15170         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15171         final Acceleration bx2 = new Acceleration(0.0,
15172                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15173         calibrator.getBiasXAsAcceleration(bx2);
15174         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15175         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15176         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
15177         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15178         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15179         final Acceleration by2 = new Acceleration(0.0,
15180                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15181         calibrator.getBiasYAsAcceleration(by2);
15182         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15183         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15184         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
15185         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15186         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15187         final Acceleration bz2 = new Acceleration(0.0,
15188                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15189         calibrator.getBiasZAsAcceleration(bz2);
15190         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15191         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15192         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15193         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15194         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15195         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15196         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15197         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15198         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15199         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15200         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15201         final double[] bias1 = calibrator.getBias();
15202         assertArrayEquals(bias1, bias, 0.0);
15203         final double[] bias2 = new double[3];
15204         calibrator.getBias(bias2);
15205         assertArrayEquals(bias1, bias2, 0.0);
15206         final Matrix b1 = calibrator.getBiasAsMatrix();
15207         assertEquals(b1, ba);
15208         final Matrix b2 = new Matrix(3, 1);
15209         calibrator.getBiasAsMatrix(b2);
15210         assertEquals(b1, b2);
15211         final Matrix ma1 = calibrator.getInitialMa();
15212         assertEquals(ma1, new Matrix(3, 3));
15213         final Matrix ma2 = new Matrix(3, 3);
15214         calibrator.getInitialMa(ma2);
15215         assertEquals(ma1, ma2);
15216         assertSame(calibrator.getMeasurements(), measurements);
15217         assertFalse(calibrator.isCommonAxisUsed());
15218         assertNull(calibrator.getListener());
15219         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15220         assertFalse(calibrator.isReady());
15221         assertFalse(calibrator.isRunning());
15222         assertNull(calibrator.getEstimatedMa());
15223         assertNull(calibrator.getEstimatedSx());
15224         assertNull(calibrator.getEstimatedSy());
15225         assertNull(calibrator.getEstimatedSz());
15226         assertNull(calibrator.getEstimatedMxy());
15227         assertNull(calibrator.getEstimatedMxz());
15228         assertNull(calibrator.getEstimatedMyx());
15229         assertNull(calibrator.getEstimatedMyz());
15230         assertNull(calibrator.getEstimatedMzx());
15231         assertNull(calibrator.getEstimatedMzy());
15232         assertNull(calibrator.getEstimatedCovariance());
15233         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15234         assertNotNull(calibrator.getGroundTruthGravityNorm());
15235         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15236         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15237         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15238                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15239         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15240         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15241         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15242 
15243         // Force IllegalArgumentException
15244         calibrator = null;
15245         try {
15246             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15247                     -gravityNorm, measurements, bias);
15248             fail("IllegalArgumentException expected but not thrown");
15249         } catch (final IllegalArgumentException ignore) {
15250         }
15251         try {
15252             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15253                     gravityNorm, measurements, new double[1]);
15254             fail("IllegalArgumentException expected but not thrown");
15255         } catch (final IllegalArgumentException ignore) {
15256         }
15257         assertNull(calibrator);
15258     }
15259 
15260     @Test
15261     public void testConstructor136() throws WrongSizeException {
15262         final Collection<StandardDeviationBodyKinematics> measurements =
15263                 Collections.emptyList();
15264 
15265         final Matrix ba = generateBa();
15266         final double[] bias = ba.getBuffer();
15267         final double biasX = ba.getElementAtIndex(0);
15268         final double biasY = ba.getElementAtIndex(1);
15269         final double biasZ = ba.getElementAtIndex(2);
15270 
15271         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15272         final double latitude = Math.toRadians(
15273                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15274         final double longitude = Math.toRadians(
15275                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15276         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15277         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15278         final NEDVelocity nedVelocity = new NEDVelocity();
15279         final ECEFPosition ecefPosition = new ECEFPosition();
15280         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15281         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15282                 ecefPosition, ecefVelocity);
15283         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15284                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15285         final double gravityNorm = gravity.getNorm();
15286 
15287         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
15288                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
15289                         measurements, bias, this);
15290 
15291         // check default values
15292         assertEquals(calibrator.getBiasX(), biasX, 0.0);
15293         assertEquals(calibrator.getBiasY(), biasY, 0.0);
15294         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
15295         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
15296         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15297         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15298         final Acceleration bx2 = new Acceleration(0.0,
15299                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15300         calibrator.getBiasXAsAcceleration(bx2);
15301         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15302         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15303         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
15304         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15305         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15306         final Acceleration by2 = new Acceleration(0.0,
15307                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15308         calibrator.getBiasYAsAcceleration(by2);
15309         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15310         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15311         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
15312         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15313         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15314         final Acceleration bz2 = new Acceleration(0.0,
15315                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15316         calibrator.getBiasZAsAcceleration(bz2);
15317         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15318         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15319         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15320         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15321         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15322         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15323         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15324         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15325         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15326         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15327         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15328         final double[] bias1 = calibrator.getBias();
15329         assertArrayEquals(bias1, bias, 0.0);
15330         final double[] bias2 = new double[3];
15331         calibrator.getBias(bias2);
15332         assertArrayEquals(bias1, bias2, 0.0);
15333         final Matrix b1 = calibrator.getBiasAsMatrix();
15334         assertEquals(b1, ba);
15335         final Matrix b2 = new Matrix(3, 1);
15336         calibrator.getBiasAsMatrix(b2);
15337         assertEquals(b1, b2);
15338         final Matrix ma1 = calibrator.getInitialMa();
15339         assertEquals(ma1, new Matrix(3, 3));
15340         final Matrix ma2 = new Matrix(3, 3);
15341         calibrator.getInitialMa(ma2);
15342         assertEquals(ma1, ma2);
15343         assertSame(calibrator.getMeasurements(), measurements);
15344         assertFalse(calibrator.isCommonAxisUsed());
15345         assertSame(calibrator.getListener(), this);
15346         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15347         assertFalse(calibrator.isReady());
15348         assertFalse(calibrator.isRunning());
15349         assertNull(calibrator.getEstimatedMa());
15350         assertNull(calibrator.getEstimatedSx());
15351         assertNull(calibrator.getEstimatedSy());
15352         assertNull(calibrator.getEstimatedSz());
15353         assertNull(calibrator.getEstimatedMxy());
15354         assertNull(calibrator.getEstimatedMxz());
15355         assertNull(calibrator.getEstimatedMyx());
15356         assertNull(calibrator.getEstimatedMyz());
15357         assertNull(calibrator.getEstimatedMzx());
15358         assertNull(calibrator.getEstimatedMzy());
15359         assertNull(calibrator.getEstimatedCovariance());
15360         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15361         assertNotNull(calibrator.getGroundTruthGravityNorm());
15362         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15363         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15364         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15365                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15366         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15367         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15368         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15369 
15370         // Force IllegalArgumentException
15371         calibrator = null;
15372         try {
15373             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15374                     -gravityNorm, measurements, bias, this);
15375             fail("IllegalArgumentException expected but not thrown");
15376         } catch (final IllegalArgumentException ignore) {
15377         }
15378         try {
15379             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15380                     gravityNorm, measurements, new double[1], this);
15381             fail("IllegalArgumentException expected but not thrown");
15382         } catch (final IllegalArgumentException ignore) {
15383         }
15384         assertNull(calibrator);
15385     }
15386 
15387     @Test
15388     public void testConstructor137() throws WrongSizeException {
15389         final Matrix ba = generateBa();
15390         final double[] bias = ba.getBuffer();
15391         final double biasX = ba.getElementAtIndex(0);
15392         final double biasY = ba.getElementAtIndex(1);
15393         final double biasZ = ba.getElementAtIndex(2);
15394 
15395         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15396         final double latitude = Math.toRadians(
15397                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15398         final double longitude = Math.toRadians(
15399                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15400         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15401         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15402         final NEDVelocity nedVelocity = new NEDVelocity();
15403         final ECEFPosition ecefPosition = new ECEFPosition();
15404         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15405         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15406                 ecefPosition, ecefVelocity);
15407         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15408                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15409         final double gravityNorm = gravity.getNorm();
15410 
15411         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
15412                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
15413                         true, bias);
15414 
15415         // check default values
15416         assertEquals(calibrator.getBiasX(), biasX, 0.0);
15417         assertEquals(calibrator.getBiasY(), biasY, 0.0);
15418         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
15419         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
15420         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15421         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15422         final Acceleration bx2 = new Acceleration(0.0,
15423                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15424         calibrator.getBiasXAsAcceleration(bx2);
15425         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15426         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15427         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
15428         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15429         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15430         final Acceleration by2 = new Acceleration(0.0,
15431                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15432         calibrator.getBiasYAsAcceleration(by2);
15433         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15434         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15435         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
15436         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15437         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15438         final Acceleration bz2 = new Acceleration(0.0,
15439                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15440         calibrator.getBiasZAsAcceleration(bz2);
15441         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15442         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15443         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15444         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15445         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15446         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15447         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15448         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15449         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15450         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15451         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15452         final double[] bias1 = calibrator.getBias();
15453         assertArrayEquals(bias1, bias, 0.0);
15454         final double[] bias2 = new double[3];
15455         calibrator.getBias(bias2);
15456         assertArrayEquals(bias1, bias2, 0.0);
15457         final Matrix b1 = calibrator.getBiasAsMatrix();
15458         assertEquals(b1, ba);
15459         final Matrix b2 = new Matrix(3, 1);
15460         calibrator.getBiasAsMatrix(b2);
15461         assertEquals(b1, b2);
15462         final Matrix ma1 = calibrator.getInitialMa();
15463         assertEquals(ma1, new Matrix(3, 3));
15464         final Matrix ma2 = new Matrix(3, 3);
15465         calibrator.getInitialMa(ma2);
15466         assertEquals(ma1, ma2);
15467         assertNull(calibrator.getMeasurements());
15468         assertTrue(calibrator.isCommonAxisUsed());
15469         assertNull(calibrator.getListener());
15470         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
15471         assertFalse(calibrator.isReady());
15472         assertFalse(calibrator.isRunning());
15473         assertNull(calibrator.getEstimatedMa());
15474         assertNull(calibrator.getEstimatedSx());
15475         assertNull(calibrator.getEstimatedSy());
15476         assertNull(calibrator.getEstimatedSz());
15477         assertNull(calibrator.getEstimatedMxy());
15478         assertNull(calibrator.getEstimatedMxz());
15479         assertNull(calibrator.getEstimatedMyx());
15480         assertNull(calibrator.getEstimatedMyz());
15481         assertNull(calibrator.getEstimatedMzx());
15482         assertNull(calibrator.getEstimatedMzy());
15483         assertNull(calibrator.getEstimatedCovariance());
15484         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15485         assertNotNull(calibrator.getGroundTruthGravityNorm());
15486         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15487         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15488         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15489                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15490         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15491         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15492         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15493 
15494         // Force IllegalArgumentException
15495         calibrator = null;
15496         try {
15497             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15498                     -gravityNorm, true, bias);
15499             fail("IllegalArgumentException expected but not thrown");
15500         } catch (final IllegalArgumentException ignore) {
15501         }
15502         try {
15503             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15504                     gravityNorm, true, new double[1]);
15505             fail("IllegalArgumentException expected but not thrown");
15506         } catch (final IllegalArgumentException ignore) {
15507         }
15508         assertNull(calibrator);
15509     }
15510 
15511     @Test
15512     public void testConstructor138() throws WrongSizeException {
15513         final Matrix ba = generateBa();
15514         final double[] bias = ba.getBuffer();
15515         final double biasX = ba.getElementAtIndex(0);
15516         final double biasY = ba.getElementAtIndex(1);
15517         final double biasZ = ba.getElementAtIndex(2);
15518 
15519         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15520         final double latitude = Math.toRadians(
15521                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15522         final double longitude = Math.toRadians(
15523                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15524         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15525         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15526         final NEDVelocity nedVelocity = new NEDVelocity();
15527         final ECEFPosition ecefPosition = new ECEFPosition();
15528         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15529         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15530                 ecefPosition, ecefVelocity);
15531         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15532                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15533         final double gravityNorm = gravity.getNorm();
15534 
15535         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
15536                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
15537                         true, bias, this);
15538 
15539         // check default values
15540         assertEquals(calibrator.getBiasX(), biasX, 0.0);
15541         assertEquals(calibrator.getBiasY(), biasY, 0.0);
15542         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
15543         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
15544         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15545         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15546         final Acceleration bx2 = new Acceleration(0.0,
15547                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15548         calibrator.getBiasXAsAcceleration(bx2);
15549         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15550         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15551         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
15552         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15553         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15554         final Acceleration by2 = new Acceleration(0.0,
15555                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15556         calibrator.getBiasYAsAcceleration(by2);
15557         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15558         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15559         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
15560         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15561         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15562         final Acceleration bz2 = new Acceleration(0.0,
15563                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15564         calibrator.getBiasZAsAcceleration(bz2);
15565         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15566         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15567         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15568         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15569         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15570         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15571         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15572         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15573         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15574         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15575         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15576         final double[] bias1 = calibrator.getBias();
15577         assertArrayEquals(bias1, bias, 0.0);
15578         final double[] bias2 = new double[3];
15579         calibrator.getBias(bias2);
15580         assertArrayEquals(bias1, bias2, 0.0);
15581         final Matrix b1 = calibrator.getBiasAsMatrix();
15582         assertEquals(b1, ba);
15583         final Matrix b2 = new Matrix(3, 1);
15584         calibrator.getBiasAsMatrix(b2);
15585         assertEquals(b1, b2);
15586         final Matrix ma1 = calibrator.getInitialMa();
15587         assertEquals(ma1, new Matrix(3, 3));
15588         final Matrix ma2 = new Matrix(3, 3);
15589         calibrator.getInitialMa(ma2);
15590         assertEquals(ma1, ma2);
15591         assertNull(calibrator.getMeasurements());
15592         assertTrue(calibrator.isCommonAxisUsed());
15593         assertSame(calibrator.getListener(), this);
15594         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
15595         assertFalse(calibrator.isReady());
15596         assertFalse(calibrator.isRunning());
15597         assertNull(calibrator.getEstimatedMa());
15598         assertNull(calibrator.getEstimatedSx());
15599         assertNull(calibrator.getEstimatedSy());
15600         assertNull(calibrator.getEstimatedSz());
15601         assertNull(calibrator.getEstimatedMxy());
15602         assertNull(calibrator.getEstimatedMxz());
15603         assertNull(calibrator.getEstimatedMyx());
15604         assertNull(calibrator.getEstimatedMyz());
15605         assertNull(calibrator.getEstimatedMzx());
15606         assertNull(calibrator.getEstimatedMzy());
15607         assertNull(calibrator.getEstimatedCovariance());
15608         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15609         assertNotNull(calibrator.getGroundTruthGravityNorm());
15610         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15611         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15612         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15613                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15614         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15615         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15616         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15617 
15618         // Force IllegalArgumentException
15619         calibrator = null;
15620         try {
15621             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15622                     -gravityNorm, true, bias, this);
15623             fail("IllegalArgumentException expected but not thrown");
15624         } catch (final IllegalArgumentException ignore) {
15625         }
15626         try {
15627             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15628                     gravityNorm, true, new double[1],
15629                     this);
15630             fail("IllegalArgumentException expected but not thrown");
15631         } catch (final IllegalArgumentException ignore) {
15632         }
15633         assertNull(calibrator);
15634     }
15635 
15636     @Test
15637     public void testConstructor139() throws WrongSizeException {
15638         final Collection<StandardDeviationBodyKinematics> measurements =
15639                 Collections.emptyList();
15640 
15641         final Matrix ba = generateBa();
15642         final double[] bias = ba.getBuffer();
15643         final double biasX = ba.getElementAtIndex(0);
15644         final double biasY = ba.getElementAtIndex(1);
15645         final double biasZ = ba.getElementAtIndex(2);
15646 
15647         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15648         final double latitude = Math.toRadians(
15649                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15650         final double longitude = Math.toRadians(
15651                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15652         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15653         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15654         final NEDVelocity nedVelocity = new NEDVelocity();
15655         final ECEFPosition ecefPosition = new ECEFPosition();
15656         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15657         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15658                 ecefPosition, ecefVelocity);
15659         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15660                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15661         final double gravityNorm = gravity.getNorm();
15662 
15663         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
15664                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
15665                         measurements, true, bias);
15666 
15667         // check default values
15668         assertEquals(calibrator.getBiasX(), biasX, 0.0);
15669         assertEquals(calibrator.getBiasY(), biasY, 0.0);
15670         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
15671         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
15672         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15673         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15674         final Acceleration bx2 = new Acceleration(0.0,
15675                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15676         calibrator.getBiasXAsAcceleration(bx2);
15677         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15678         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15679         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
15680         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15681         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15682         final Acceleration by2 = new Acceleration(0.0,
15683                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15684         calibrator.getBiasYAsAcceleration(by2);
15685         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15686         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15687         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
15688         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15689         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15690         final Acceleration bz2 = new Acceleration(0.0,
15691                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15692         calibrator.getBiasZAsAcceleration(bz2);
15693         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15694         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15695         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15696         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15697         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15698         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15699         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15700         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15701         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15702         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15703         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15704         final double[] bias1 = calibrator.getBias();
15705         assertArrayEquals(bias1, bias, 0.0);
15706         final double[] bias2 = new double[3];
15707         calibrator.getBias(bias2);
15708         assertArrayEquals(bias1, bias2, 0.0);
15709         final Matrix b1 = calibrator.getBiasAsMatrix();
15710         assertEquals(b1, ba);
15711         final Matrix b2 = new Matrix(3, 1);
15712         calibrator.getBiasAsMatrix(b2);
15713         assertEquals(b1, b2);
15714         final Matrix ma1 = calibrator.getInitialMa();
15715         assertEquals(ma1, new Matrix(3, 3));
15716         final Matrix ma2 = new Matrix(3, 3);
15717         calibrator.getInitialMa(ma2);
15718         assertEquals(ma1, ma2);
15719         assertSame(calibrator.getMeasurements(), measurements);
15720         assertTrue(calibrator.isCommonAxisUsed());
15721         assertNull(calibrator.getListener());
15722         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
15723         assertFalse(calibrator.isReady());
15724         assertFalse(calibrator.isRunning());
15725         assertNull(calibrator.getEstimatedMa());
15726         assertNull(calibrator.getEstimatedSx());
15727         assertNull(calibrator.getEstimatedSy());
15728         assertNull(calibrator.getEstimatedSz());
15729         assertNull(calibrator.getEstimatedMxy());
15730         assertNull(calibrator.getEstimatedMxz());
15731         assertNull(calibrator.getEstimatedMyx());
15732         assertNull(calibrator.getEstimatedMyz());
15733         assertNull(calibrator.getEstimatedMzx());
15734         assertNull(calibrator.getEstimatedMzy());
15735         assertNull(calibrator.getEstimatedCovariance());
15736         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15737         assertNotNull(calibrator.getGroundTruthGravityNorm());
15738         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15739         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15740         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15741                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15742         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15743         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15744         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15745 
15746         // Force IllegalArgumentException
15747         calibrator = null;
15748         try {
15749             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15750                     -gravityNorm, measurements, true, bias);
15751             fail("IllegalArgumentException expected but not thrown");
15752         } catch (final IllegalArgumentException ignore) {
15753         }
15754         try {
15755             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15756                     gravityNorm, measurements, true,
15757                     new double[1]);
15758             fail("IllegalArgumentException expected but not thrown");
15759         } catch (final IllegalArgumentException ignore) {
15760         }
15761         assertNull(calibrator);
15762     }
15763 
15764     @Test
15765     public void testConstructor140() throws WrongSizeException {
15766         final Collection<StandardDeviationBodyKinematics> measurements =
15767                 Collections.emptyList();
15768 
15769         final Matrix ba = generateBa();
15770         final double[] bias = ba.getBuffer();
15771         final double biasX = ba.getElementAtIndex(0);
15772         final double biasY = ba.getElementAtIndex(1);
15773         final double biasZ = ba.getElementAtIndex(2);
15774 
15775         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15776         final double latitude = Math.toRadians(
15777                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15778         final double longitude = Math.toRadians(
15779                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15780         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15781         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15782         final NEDVelocity nedVelocity = new NEDVelocity();
15783         final ECEFPosition ecefPosition = new ECEFPosition();
15784         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15785         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15786                 ecefPosition, ecefVelocity);
15787         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15788                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15789         final double gravityNorm = gravity.getNorm();
15790 
15791         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
15792                 new KnownBiasAndGravityNormAccelerometerCalibrator(
15793                         gravityNorm, measurements,
15794                         true, bias, this);
15795 
15796         // check default values
15797         assertEquals(calibrator.getBiasX(), biasX, 0.0);
15798         assertEquals(calibrator.getBiasY(), biasY, 0.0);
15799         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
15800         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
15801         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15802         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15803         final Acceleration bx2 = new Acceleration(0.0,
15804                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15805         calibrator.getBiasXAsAcceleration(bx2);
15806         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15807         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15808         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
15809         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15810         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15811         final Acceleration by2 = new Acceleration(0.0,
15812                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15813         calibrator.getBiasYAsAcceleration(by2);
15814         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15815         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15816         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
15817         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15818         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15819         final Acceleration bz2 = new Acceleration(0.0,
15820                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15821         calibrator.getBiasZAsAcceleration(bz2);
15822         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15823         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15824         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15825         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15826         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15827         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15828         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15829         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15830         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15831         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15832         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15833         final double[] bias1 = calibrator.getBias();
15834         assertArrayEquals(bias1, bias, 0.0);
15835         final double[] bias2 = new double[3];
15836         calibrator.getBias(bias2);
15837         assertArrayEquals(bias1, bias2, 0.0);
15838         final Matrix b1 = calibrator.getBiasAsMatrix();
15839         assertEquals(b1, ba);
15840         final Matrix b2 = new Matrix(3, 1);
15841         calibrator.getBiasAsMatrix(b2);
15842         assertEquals(b1, b2);
15843         final Matrix ma1 = calibrator.getInitialMa();
15844         assertEquals(ma1, new Matrix(3, 3));
15845         final Matrix ma2 = new Matrix(3, 3);
15846         calibrator.getInitialMa(ma2);
15847         assertEquals(ma1, ma2);
15848         assertSame(calibrator.getMeasurements(), measurements);
15849         assertTrue(calibrator.isCommonAxisUsed());
15850         assertSame(calibrator.getListener(), this);
15851         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
15852         assertFalse(calibrator.isReady());
15853         assertFalse(calibrator.isRunning());
15854         assertNull(calibrator.getEstimatedMa());
15855         assertNull(calibrator.getEstimatedSx());
15856         assertNull(calibrator.getEstimatedSy());
15857         assertNull(calibrator.getEstimatedSz());
15858         assertNull(calibrator.getEstimatedMxy());
15859         assertNull(calibrator.getEstimatedMxz());
15860         assertNull(calibrator.getEstimatedMyx());
15861         assertNull(calibrator.getEstimatedMyz());
15862         assertNull(calibrator.getEstimatedMzx());
15863         assertNull(calibrator.getEstimatedMzy());
15864         assertNull(calibrator.getEstimatedCovariance());
15865         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15866         assertNotNull(calibrator.getGroundTruthGravityNorm());
15867         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15868         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15869         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15870                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15871         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15872         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15873         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15874 
15875         // Force IllegalArgumentException
15876         calibrator = null;
15877         try {
15878             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15879                     -gravityNorm, measurements,
15880                     true, bias, this);
15881             fail("IllegalArgumentException expected but not thrown");
15882         } catch (final IllegalArgumentException ignore) {
15883         }
15884         try {
15885             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
15886                     gravityNorm, measurements, true,
15887                     new double[1], this);
15888             fail("IllegalArgumentException expected but not thrown");
15889         } catch (final IllegalArgumentException ignore) {
15890         }
15891         assertNull(calibrator);
15892     }
15893 
15894     @Test
15895     public void testConstructor141() throws WrongSizeException {
15896         final Matrix ba = generateBa();
15897         final double[] bias = ba.getBuffer();
15898         final double biasX = ba.getElementAtIndex(0);
15899         final double biasY = ba.getElementAtIndex(1);
15900         final double biasZ = ba.getElementAtIndex(2);
15901 
15902         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15903         final double latitude = Math.toRadians(
15904                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15905         final double longitude = Math.toRadians(
15906                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15907         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15908         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15909         final NEDVelocity nedVelocity = new NEDVelocity();
15910         final ECEFPosition ecefPosition = new ECEFPosition();
15911         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15912         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15913                 ecefPosition, ecefVelocity);
15914         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15915                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15916         final double gravityNorm = gravity.getNorm();
15917 
15918         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
15919                 new KnownBiasAndGravityNormAccelerometerCalibrator(
15920                         gravityNorm, ba);
15921 
15922         // check default values
15923         assertEquals(calibrator.getBiasX(), biasX, 0.0);
15924         assertEquals(calibrator.getBiasY(), biasY, 0.0);
15925         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
15926         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
15927         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15928         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15929         final Acceleration bx2 = new Acceleration(0.0,
15930                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15931         calibrator.getBiasXAsAcceleration(bx2);
15932         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15933         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15934         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
15935         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15936         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15937         final Acceleration by2 = new Acceleration(0.0,
15938                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15939         calibrator.getBiasYAsAcceleration(by2);
15940         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15941         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15942         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
15943         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15944         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15945         final Acceleration bz2 = new Acceleration(0.0,
15946                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15947         calibrator.getBiasZAsAcceleration(bz2);
15948         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15949         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15950         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15951         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15952         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15953         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15954         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15955         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15956         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15957         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15958         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15959         final double[] bias1 = calibrator.getBias();
15960         assertArrayEquals(bias1, bias, 0.0);
15961         final double[] bias2 = new double[3];
15962         calibrator.getBias(bias2);
15963         assertArrayEquals(bias1, bias2, 0.0);
15964         final Matrix b1 = calibrator.getBiasAsMatrix();
15965         assertEquals(b1, ba);
15966         final Matrix b2 = new Matrix(3, 1);
15967         calibrator.getBiasAsMatrix(b2);
15968         assertEquals(b1, b2);
15969         final Matrix ma1 = calibrator.getInitialMa();
15970         assertEquals(ma1, new Matrix(3, 3));
15971         final Matrix ma2 = new Matrix(3, 3);
15972         calibrator.getInitialMa(ma2);
15973         assertEquals(ma1, ma2);
15974         assertNull(calibrator.getMeasurements());
15975         assertFalse(calibrator.isCommonAxisUsed());
15976         assertNull(calibrator.getListener());
15977         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15978         assertFalse(calibrator.isReady());
15979         assertFalse(calibrator.isRunning());
15980         assertNull(calibrator.getEstimatedMa());
15981         assertNull(calibrator.getEstimatedSx());
15982         assertNull(calibrator.getEstimatedSy());
15983         assertNull(calibrator.getEstimatedSz());
15984         assertNull(calibrator.getEstimatedMxy());
15985         assertNull(calibrator.getEstimatedMxz());
15986         assertNull(calibrator.getEstimatedMyx());
15987         assertNull(calibrator.getEstimatedMyz());
15988         assertNull(calibrator.getEstimatedMzx());
15989         assertNull(calibrator.getEstimatedMzy());
15990         assertNull(calibrator.getEstimatedCovariance());
15991         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15992         assertNotNull(calibrator.getGroundTruthGravityNorm());
15993         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15994         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15995         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15996                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15997         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15998         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15999         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16000 
16001         // Force IllegalArgumentException
16002         calibrator = null;
16003         try {
16004             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16005                     -gravityNorm, ba);
16006             fail("IllegalArgumentException expected but not thrown");
16007         } catch (final IllegalArgumentException ignore) {
16008         }
16009         try {
16010             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16011                     gravityNorm, new Matrix(1, 1));
16012             fail("IllegalArgumentException expected but not thrown");
16013         } catch (final IllegalArgumentException ignore) {
16014         }
16015         try {
16016             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16017                     gravityNorm, new Matrix(1, 3));
16018             fail("IllegalArgumentException expected but not thrown");
16019         } catch (final IllegalArgumentException ignore) {
16020         }
16021         assertNull(calibrator);
16022     }
16023 
16024     @Test
16025     public void testConstructor142() throws WrongSizeException {
16026         final Matrix ba = generateBa();
16027         final double[] bias = ba.getBuffer();
16028         final double biasX = ba.getElementAtIndex(0);
16029         final double biasY = ba.getElementAtIndex(1);
16030         final double biasZ = ba.getElementAtIndex(2);
16031 
16032         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16033         final double latitude = Math.toRadians(
16034                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16035         final double longitude = Math.toRadians(
16036                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16037         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16038         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16039         final NEDVelocity nedVelocity = new NEDVelocity();
16040         final ECEFPosition ecefPosition = new ECEFPosition();
16041         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16042         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16043                 ecefPosition, ecefVelocity);
16044         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16045                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16046         final double gravityNorm = gravity.getNorm();
16047 
16048         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
16049                 new KnownBiasAndGravityNormAccelerometerCalibrator(
16050                         gravityNorm, ba, this);
16051 
16052         // check default values
16053         assertEquals(calibrator.getBiasX(), biasX, 0.0);
16054         assertEquals(calibrator.getBiasY(), biasY, 0.0);
16055         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
16056         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
16057         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16058         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16059         final Acceleration bx2 = new Acceleration(0.0,
16060                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16061         calibrator.getBiasXAsAcceleration(bx2);
16062         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16063         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16064         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
16065         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16066         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16067         final Acceleration by2 = new Acceleration(0.0,
16068                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16069         calibrator.getBiasYAsAcceleration(by2);
16070         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16071         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16072         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
16073         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16074         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16075         final Acceleration bz2 = new Acceleration(0.0,
16076                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16077         calibrator.getBiasZAsAcceleration(bz2);
16078         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16079         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16080         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16081         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16082         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16083         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16084         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16085         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16086         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16087         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16088         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16089         final double[] bias1 = calibrator.getBias();
16090         assertArrayEquals(bias1, bias, 0.0);
16091         final double[] bias2 = new double[3];
16092         calibrator.getBias(bias2);
16093         assertArrayEquals(bias1, bias2, 0.0);
16094         final Matrix b1 = calibrator.getBiasAsMatrix();
16095         assertEquals(b1, ba);
16096         final Matrix b2 = new Matrix(3, 1);
16097         calibrator.getBiasAsMatrix(b2);
16098         assertEquals(b1, b2);
16099         final Matrix ma1 = calibrator.getInitialMa();
16100         assertEquals(ma1, new Matrix(3, 3));
16101         final Matrix ma2 = new Matrix(3, 3);
16102         calibrator.getInitialMa(ma2);
16103         assertEquals(ma1, ma2);
16104         assertNull(calibrator.getMeasurements());
16105         assertFalse(calibrator.isCommonAxisUsed());
16106         assertSame(calibrator.getListener(), this);
16107         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16108         assertFalse(calibrator.isReady());
16109         assertFalse(calibrator.isRunning());
16110         assertNull(calibrator.getEstimatedMa());
16111         assertNull(calibrator.getEstimatedSx());
16112         assertNull(calibrator.getEstimatedSy());
16113         assertNull(calibrator.getEstimatedSz());
16114         assertNull(calibrator.getEstimatedMxy());
16115         assertNull(calibrator.getEstimatedMxz());
16116         assertNull(calibrator.getEstimatedMyx());
16117         assertNull(calibrator.getEstimatedMyz());
16118         assertNull(calibrator.getEstimatedMzx());
16119         assertNull(calibrator.getEstimatedMzy());
16120         assertNull(calibrator.getEstimatedCovariance());
16121         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16122         assertNotNull(calibrator.getGroundTruthGravityNorm());
16123         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16124         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16125         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16126                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16127         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16128         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16129         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16130 
16131         // Force IllegalArgumentException
16132         calibrator = null;
16133         try {
16134             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16135                     -gravityNorm, ba, this);
16136             fail("IllegalArgumentException expected but not thrown");
16137         } catch (final IllegalArgumentException ignore) {
16138         }
16139         try {
16140             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16141                     gravityNorm, new Matrix(1, 1), this);
16142             fail("IllegalArgumentException expected but not thrown");
16143         } catch (final IllegalArgumentException ignore) {
16144         }
16145         try {
16146             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16147                     gravityNorm, new Matrix(1, 3),
16148                     this);
16149             fail("IllegalArgumentException expected but not thrown");
16150         } catch (final IllegalArgumentException ignore) {
16151         }
16152         assertNull(calibrator);
16153     }
16154 
16155     @Test
16156     public void testConstructor143() throws WrongSizeException {
16157         final Collection<StandardDeviationBodyKinematics> measurements =
16158                 Collections.emptyList();
16159 
16160         final Matrix ba = generateBa();
16161         final double[] bias = ba.getBuffer();
16162         final double biasX = ba.getElementAtIndex(0);
16163         final double biasY = ba.getElementAtIndex(1);
16164         final double biasZ = ba.getElementAtIndex(2);
16165 
16166         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16167         final double latitude = Math.toRadians(
16168                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16169         final double longitude = Math.toRadians(
16170                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16171         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16172         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16173         final NEDVelocity nedVelocity = new NEDVelocity();
16174         final ECEFPosition ecefPosition = new ECEFPosition();
16175         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16176         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16177                 ecefPosition, ecefVelocity);
16178         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16179                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16180         final double gravityNorm = gravity.getNorm();
16181 
16182         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
16183                 new KnownBiasAndGravityNormAccelerometerCalibrator(
16184                         gravityNorm, measurements, ba);
16185 
16186         // check default values
16187         assertEquals(calibrator.getBiasX(), biasX, 0.0);
16188         assertEquals(calibrator.getBiasY(), biasY, 0.0);
16189         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
16190         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
16191         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16192         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16193         final Acceleration bx2 = new Acceleration(0.0,
16194                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16195         calibrator.getBiasXAsAcceleration(bx2);
16196         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16197         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16198         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
16199         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16200         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16201         final Acceleration by2 = new Acceleration(0.0,
16202                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16203         calibrator.getBiasYAsAcceleration(by2);
16204         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16205         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16206         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
16207         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16208         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16209         final Acceleration bz2 = new Acceleration(0.0,
16210                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16211         calibrator.getBiasZAsAcceleration(bz2);
16212         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16213         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16214         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16215         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16216         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16217         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16218         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16219         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16220         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16221         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16222         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16223         final double[] bias1 = calibrator.getBias();
16224         assertArrayEquals(bias1, bias, 0.0);
16225         final double[] bias2 = new double[3];
16226         calibrator.getBias(bias2);
16227         assertArrayEquals(bias1, bias2, 0.0);
16228         final Matrix b1 = calibrator.getBiasAsMatrix();
16229         assertEquals(b1, ba);
16230         final Matrix b2 = new Matrix(3, 1);
16231         calibrator.getBiasAsMatrix(b2);
16232         assertEquals(b1, b2);
16233         final Matrix ma1 = calibrator.getInitialMa();
16234         assertEquals(ma1, new Matrix(3, 3));
16235         final Matrix ma2 = new Matrix(3, 3);
16236         calibrator.getInitialMa(ma2);
16237         assertEquals(ma1, ma2);
16238         assertSame(calibrator.getMeasurements(), measurements);
16239         assertFalse(calibrator.isCommonAxisUsed());
16240         assertNull(calibrator.getListener());
16241         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16242         assertFalse(calibrator.isReady());
16243         assertFalse(calibrator.isRunning());
16244         assertNull(calibrator.getEstimatedMa());
16245         assertNull(calibrator.getEstimatedSx());
16246         assertNull(calibrator.getEstimatedSy());
16247         assertNull(calibrator.getEstimatedSz());
16248         assertNull(calibrator.getEstimatedMxy());
16249         assertNull(calibrator.getEstimatedMxz());
16250         assertNull(calibrator.getEstimatedMyx());
16251         assertNull(calibrator.getEstimatedMyz());
16252         assertNull(calibrator.getEstimatedMzx());
16253         assertNull(calibrator.getEstimatedMzy());
16254         assertNull(calibrator.getEstimatedCovariance());
16255         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16256         assertNotNull(calibrator.getGroundTruthGravityNorm());
16257         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16258         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16259         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16260                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16261         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16262         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16263         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16264 
16265         // Force IllegalArgumentException
16266         calibrator = null;
16267         try {
16268             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16269                     -gravityNorm, measurements, ba);
16270             fail("IllegalArgumentException expected but not thrown");
16271         } catch (final IllegalArgumentException ignore) {
16272         }
16273         try {
16274             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16275                     gravityNorm, measurements,
16276                     new Matrix(1, 1));
16277             fail("IllegalArgumentException expected but not thrown");
16278         } catch (final IllegalArgumentException ignore) {
16279         }
16280         try {
16281             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16282                     gravityNorm, measurements,
16283                     new Matrix(1, 3));
16284             fail("IllegalArgumentException expected but not thrown");
16285         } catch (final IllegalArgumentException ignore) {
16286         }
16287         assertNull(calibrator);
16288     }
16289 
16290     @Test
16291     public void testConstructor144() throws WrongSizeException {
16292         final Collection<StandardDeviationBodyKinematics> measurements =
16293                 Collections.emptyList();
16294 
16295         final Matrix ba = generateBa();
16296         final double[] bias = ba.getBuffer();
16297         final double biasX = ba.getElementAtIndex(0);
16298         final double biasY = ba.getElementAtIndex(1);
16299         final double biasZ = ba.getElementAtIndex(2);
16300 
16301         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16302         final double latitude = Math.toRadians(
16303                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16304         final double longitude = Math.toRadians(
16305                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16306         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16307         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16308         final NEDVelocity nedVelocity = new NEDVelocity();
16309         final ECEFPosition ecefPosition = new ECEFPosition();
16310         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16311         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16312                 ecefPosition, ecefVelocity);
16313         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16314                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16315         final double gravityNorm = gravity.getNorm();
16316 
16317         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
16318                 new KnownBiasAndGravityNormAccelerometerCalibrator(
16319                         gravityNorm, measurements, ba, this);
16320 
16321         // check default values
16322         assertEquals(calibrator.getBiasX(), biasX, 0.0);
16323         assertEquals(calibrator.getBiasY(), biasY, 0.0);
16324         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
16325         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
16326         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16327         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16328         final Acceleration bx2 = new Acceleration(0.0,
16329                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16330         calibrator.getBiasXAsAcceleration(bx2);
16331         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16332         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16333         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
16334         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16335         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16336         final Acceleration by2 = new Acceleration(0.0,
16337                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16338         calibrator.getBiasYAsAcceleration(by2);
16339         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16340         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16341         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
16342         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16343         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16344         final Acceleration bz2 = new Acceleration(0.0,
16345                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16346         calibrator.getBiasZAsAcceleration(bz2);
16347         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16348         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16349         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16350         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16351         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16352         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16353         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16354         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16355         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16356         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16357         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16358         final double[] bias1 = calibrator.getBias();
16359         assertArrayEquals(bias1, bias, 0.0);
16360         final double[] bias2 = new double[3];
16361         calibrator.getBias(bias2);
16362         assertArrayEquals(bias1, bias2, 0.0);
16363         final Matrix b1 = calibrator.getBiasAsMatrix();
16364         assertEquals(b1, ba);
16365         final Matrix b2 = new Matrix(3, 1);
16366         calibrator.getBiasAsMatrix(b2);
16367         assertEquals(b1, b2);
16368         final Matrix ma1 = calibrator.getInitialMa();
16369         assertEquals(ma1, new Matrix(3, 3));
16370         final Matrix ma2 = new Matrix(3, 3);
16371         calibrator.getInitialMa(ma2);
16372         assertEquals(ma1, ma2);
16373         assertSame(calibrator.getMeasurements(), measurements);
16374         assertFalse(calibrator.isCommonAxisUsed());
16375         assertSame(calibrator.getListener(), this);
16376         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16377         assertFalse(calibrator.isReady());
16378         assertFalse(calibrator.isRunning());
16379         assertNull(calibrator.getEstimatedMa());
16380         assertNull(calibrator.getEstimatedSx());
16381         assertNull(calibrator.getEstimatedSy());
16382         assertNull(calibrator.getEstimatedSz());
16383         assertNull(calibrator.getEstimatedMxy());
16384         assertNull(calibrator.getEstimatedMxz());
16385         assertNull(calibrator.getEstimatedMyx());
16386         assertNull(calibrator.getEstimatedMyz());
16387         assertNull(calibrator.getEstimatedMzx());
16388         assertNull(calibrator.getEstimatedMzy());
16389         assertNull(calibrator.getEstimatedCovariance());
16390         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16391         assertNotNull(calibrator.getGroundTruthGravityNorm());
16392         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16393         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16394         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16395                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16396         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16397         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16398         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16399 
16400         // Force IllegalArgumentException
16401         calibrator = null;
16402         try {
16403             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16404                     -gravityNorm, measurements, ba, this);
16405             fail("IllegalArgumentException expected but not thrown");
16406         } catch (final IllegalArgumentException ignore) {
16407         }
16408         try {
16409             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16410                     gravityNorm, measurements,
16411                     new Matrix(1, 1), this);
16412             fail("IllegalArgumentException expected but not thrown");
16413         } catch (final IllegalArgumentException ignore) {
16414         }
16415         try {
16416             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16417                     gravityNorm, measurements,
16418                     new Matrix(1, 3), this);
16419             fail("IllegalArgumentException expected but not thrown");
16420         } catch (final IllegalArgumentException ignore) {
16421         }
16422         assertNull(calibrator);
16423     }
16424 
16425     @Test
16426     public void testConstructor145() throws WrongSizeException {
16427         final Matrix ba = generateBa();
16428         final double[] bias = ba.getBuffer();
16429         final double biasX = ba.getElementAtIndex(0);
16430         final double biasY = ba.getElementAtIndex(1);
16431         final double biasZ = ba.getElementAtIndex(2);
16432 
16433         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16434         final double latitude = Math.toRadians(
16435                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16436         final double longitude = Math.toRadians(
16437                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16438         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16439         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16440         final NEDVelocity nedVelocity = new NEDVelocity();
16441         final ECEFPosition ecefPosition = new ECEFPosition();
16442         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16443         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16444                 ecefPosition, ecefVelocity);
16445         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16446                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16447         final double gravityNorm = gravity.getNorm();
16448 
16449         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
16450                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
16451                         true, ba);
16452 
16453         // check default values
16454         assertEquals(calibrator.getBiasX(), biasX, 0.0);
16455         assertEquals(calibrator.getBiasY(), biasY, 0.0);
16456         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
16457         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
16458         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16459         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16460         final Acceleration bx2 = new Acceleration(0.0,
16461                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16462         calibrator.getBiasXAsAcceleration(bx2);
16463         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16464         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16465         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
16466         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16467         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16468         final Acceleration by2 = new Acceleration(0.0,
16469                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16470         calibrator.getBiasYAsAcceleration(by2);
16471         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16472         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16473         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
16474         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16475         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16476         final Acceleration bz2 = new Acceleration(0.0,
16477                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16478         calibrator.getBiasZAsAcceleration(bz2);
16479         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16480         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16481         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16482         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16483         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16484         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16485         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16486         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16487         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16488         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16489         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16490         final double[] bias1 = calibrator.getBias();
16491         assertArrayEquals(bias1, bias, 0.0);
16492         final double[] bias2 = new double[3];
16493         calibrator.getBias(bias2);
16494         assertArrayEquals(bias1, bias2, 0.0);
16495         final Matrix b1 = calibrator.getBiasAsMatrix();
16496         assertEquals(b1, ba);
16497         final Matrix b2 = new Matrix(3, 1);
16498         calibrator.getBiasAsMatrix(b2);
16499         assertEquals(b1, b2);
16500         final Matrix ma1 = calibrator.getInitialMa();
16501         assertEquals(ma1, new Matrix(3, 3));
16502         final Matrix ma2 = new Matrix(3, 3);
16503         calibrator.getInitialMa(ma2);
16504         assertEquals(ma1, ma2);
16505         assertNull(calibrator.getMeasurements());
16506         assertTrue(calibrator.isCommonAxisUsed());
16507         assertNull(calibrator.getListener());
16508         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
16509         assertFalse(calibrator.isReady());
16510         assertFalse(calibrator.isRunning());
16511         assertNull(calibrator.getEstimatedMa());
16512         assertNull(calibrator.getEstimatedSx());
16513         assertNull(calibrator.getEstimatedSy());
16514         assertNull(calibrator.getEstimatedSz());
16515         assertNull(calibrator.getEstimatedMxy());
16516         assertNull(calibrator.getEstimatedMxz());
16517         assertNull(calibrator.getEstimatedMyx());
16518         assertNull(calibrator.getEstimatedMyz());
16519         assertNull(calibrator.getEstimatedMzx());
16520         assertNull(calibrator.getEstimatedMzy());
16521         assertNull(calibrator.getEstimatedCovariance());
16522         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16523         assertNotNull(calibrator.getGroundTruthGravityNorm());
16524         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16525         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16526         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16527                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16528         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16529         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16530         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16531 
16532         // Force IllegalArgumentException
16533         calibrator = null;
16534         try {
16535             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16536                     -gravityNorm, true, ba);
16537             fail("IllegalArgumentException expected but not thrown");
16538         } catch (final IllegalArgumentException ignore) {
16539         }
16540         try {
16541             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16542                     gravityNorm, true,
16543                     new Matrix(1, 1));
16544             fail("IllegalArgumentException expected but not thrown");
16545         } catch (final IllegalArgumentException ignore) {
16546         }
16547         try {
16548             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16549                     gravityNorm, true,
16550                     new Matrix(1, 3));
16551             fail("IllegalArgumentException expected but not thrown");
16552         } catch (final IllegalArgumentException ignore) {
16553         }
16554         assertNull(calibrator);
16555     }
16556 
16557     @Test
16558     public void testConstructor146() throws WrongSizeException {
16559         final Matrix ba = generateBa();
16560         final double[] bias = ba.getBuffer();
16561         final double biasX = ba.getElementAtIndex(0);
16562         final double biasY = ba.getElementAtIndex(1);
16563         final double biasZ = ba.getElementAtIndex(2);
16564 
16565         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16566         final double latitude = Math.toRadians(
16567                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16568         final double longitude = Math.toRadians(
16569                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16570         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16571         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16572         final NEDVelocity nedVelocity = new NEDVelocity();
16573         final ECEFPosition ecefPosition = new ECEFPosition();
16574         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16575         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16576                 ecefPosition, ecefVelocity);
16577         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16578                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16579         final double gravityNorm = gravity.getNorm();
16580 
16581         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
16582                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
16583                         true, ba, this);
16584 
16585         // check default values
16586         assertEquals(calibrator.getBiasX(), biasX, 0.0);
16587         assertEquals(calibrator.getBiasY(), biasY, 0.0);
16588         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
16589         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
16590         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16591         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16592         final Acceleration bx2 = new Acceleration(0.0,
16593                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16594         calibrator.getBiasXAsAcceleration(bx2);
16595         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16596         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16597         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
16598         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16599         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16600         final Acceleration by2 = new Acceleration(0.0,
16601                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16602         calibrator.getBiasYAsAcceleration(by2);
16603         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16604         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16605         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
16606         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16607         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16608         final Acceleration bz2 = new Acceleration(0.0,
16609                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16610         calibrator.getBiasZAsAcceleration(bz2);
16611         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16612         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16613         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16614         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16615         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16616         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16617         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16618         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16619         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16620         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16621         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16622         final double[] bias1 = calibrator.getBias();
16623         assertArrayEquals(bias1, bias, 0.0);
16624         final double[] bias2 = new double[3];
16625         calibrator.getBias(bias2);
16626         assertArrayEquals(bias1, bias2, 0.0);
16627         final Matrix b1 = calibrator.getBiasAsMatrix();
16628         assertEquals(b1, ba);
16629         final Matrix b2 = new Matrix(3, 1);
16630         calibrator.getBiasAsMatrix(b2);
16631         assertEquals(b1, b2);
16632         final Matrix ma1 = calibrator.getInitialMa();
16633         assertEquals(ma1, new Matrix(3, 3));
16634         final Matrix ma2 = new Matrix(3, 3);
16635         calibrator.getInitialMa(ma2);
16636         assertEquals(ma1, ma2);
16637         assertNull(calibrator.getMeasurements());
16638         assertTrue(calibrator.isCommonAxisUsed());
16639         assertSame(calibrator.getListener(), this);
16640         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
16641         assertFalse(calibrator.isReady());
16642         assertFalse(calibrator.isRunning());
16643         assertNull(calibrator.getEstimatedMa());
16644         assertNull(calibrator.getEstimatedSx());
16645         assertNull(calibrator.getEstimatedSy());
16646         assertNull(calibrator.getEstimatedSz());
16647         assertNull(calibrator.getEstimatedMxy());
16648         assertNull(calibrator.getEstimatedMxz());
16649         assertNull(calibrator.getEstimatedMyx());
16650         assertNull(calibrator.getEstimatedMyz());
16651         assertNull(calibrator.getEstimatedMzx());
16652         assertNull(calibrator.getEstimatedMzy());
16653         assertNull(calibrator.getEstimatedCovariance());
16654         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16655         assertNotNull(calibrator.getGroundTruthGravityNorm());
16656         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16657         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16658         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16659                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16660         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16661         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16662         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16663 
16664         // Force IllegalArgumentException
16665         calibrator = null;
16666         try {
16667             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16668                     -gravityNorm, true, ba, this);
16669             fail("IllegalArgumentException expected but not thrown");
16670         } catch (final IllegalArgumentException ignore) {
16671         }
16672         try {
16673             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16674                     gravityNorm, true, new Matrix(1, 1),
16675                     this);
16676             fail("IllegalArgumentException expected but not thrown");
16677         } catch (final IllegalArgumentException ignore) {
16678         }
16679         try {
16680             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16681                     gravityNorm, true, new Matrix(1, 3),
16682                     this);
16683             fail("IllegalArgumentException expected but not thrown");
16684         } catch (final IllegalArgumentException ignore) {
16685         }
16686         assertNull(calibrator);
16687     }
16688 
16689     @Test
16690     public void testConstructor147() throws WrongSizeException {
16691         final Collection<StandardDeviationBodyKinematics> measurements =
16692                 Collections.emptyList();
16693 
16694         final Matrix ba = generateBa();
16695         final double[] bias = ba.getBuffer();
16696         final double biasX = ba.getElementAtIndex(0);
16697         final double biasY = ba.getElementAtIndex(1);
16698         final double biasZ = ba.getElementAtIndex(2);
16699 
16700         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16701         final double latitude = Math.toRadians(
16702                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16703         final double longitude = Math.toRadians(
16704                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16705         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16706         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16707         final NEDVelocity nedVelocity = new NEDVelocity();
16708         final ECEFPosition ecefPosition = new ECEFPosition();
16709         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16710         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16711                 ecefPosition, ecefVelocity);
16712         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16713                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16714         final double gravityNorm = gravity.getNorm();
16715 
16716         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
16717                 new KnownBiasAndGravityNormAccelerometerCalibrator(
16718                         gravityNorm, measurements,
16719                         true, ba);
16720 
16721         // check default values
16722         assertEquals(calibrator.getBiasX(), biasX, 0.0);
16723         assertEquals(calibrator.getBiasY(), biasY, 0.0);
16724         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
16725         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
16726         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16727         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16728         final Acceleration bx2 = new Acceleration(0.0,
16729                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16730         calibrator.getBiasXAsAcceleration(bx2);
16731         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16732         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16733         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
16734         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16735         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16736         final Acceleration by2 = new Acceleration(0.0,
16737                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16738         calibrator.getBiasYAsAcceleration(by2);
16739         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16740         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16741         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
16742         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16743         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16744         final Acceleration bz2 = new Acceleration(0.0,
16745                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16746         calibrator.getBiasZAsAcceleration(bz2);
16747         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16748         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16749         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16750         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16751         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16752         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16753         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16754         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16755         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16756         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16757         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16758         final double[] bias1 = calibrator.getBias();
16759         assertArrayEquals(bias1, bias, 0.0);
16760         final double[] bias2 = new double[3];
16761         calibrator.getBias(bias2);
16762         assertArrayEquals(bias1, bias2, 0.0);
16763         final Matrix b1 = calibrator.getBiasAsMatrix();
16764         assertEquals(b1, ba);
16765         final Matrix b2 = new Matrix(3, 1);
16766         calibrator.getBiasAsMatrix(b2);
16767         assertEquals(b1, b2);
16768         final Matrix ma1 = calibrator.getInitialMa();
16769         assertEquals(ma1, new Matrix(3, 3));
16770         final Matrix ma2 = new Matrix(3, 3);
16771         calibrator.getInitialMa(ma2);
16772         assertEquals(ma1, ma2);
16773         assertSame(calibrator.getMeasurements(), measurements);
16774         assertTrue(calibrator.isCommonAxisUsed());
16775         assertNull(calibrator.getListener());
16776         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
16777         assertFalse(calibrator.isReady());
16778         assertFalse(calibrator.isRunning());
16779         assertNull(calibrator.getEstimatedMa());
16780         assertNull(calibrator.getEstimatedSx());
16781         assertNull(calibrator.getEstimatedSy());
16782         assertNull(calibrator.getEstimatedSz());
16783         assertNull(calibrator.getEstimatedMxy());
16784         assertNull(calibrator.getEstimatedMxz());
16785         assertNull(calibrator.getEstimatedMyx());
16786         assertNull(calibrator.getEstimatedMyz());
16787         assertNull(calibrator.getEstimatedMzx());
16788         assertNull(calibrator.getEstimatedMzy());
16789         assertNull(calibrator.getEstimatedCovariance());
16790         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16791         assertNotNull(calibrator.getGroundTruthGravityNorm());
16792         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16793         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16794         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16795                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16796         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16797         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16798         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16799 
16800         // Force IllegalArgumentException
16801         calibrator = null;
16802         try {
16803             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16804                     -gravityNorm, measurements,
16805                     true, ba);
16806             fail("IllegalArgumentException expected but not thrown");
16807         } catch (final IllegalArgumentException ignore) {
16808         }
16809         try {
16810             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16811                     gravityNorm, measurements, true,
16812                     new Matrix(1, 1));
16813             fail("IllegalArgumentException expected but not thrown");
16814         } catch (final IllegalArgumentException ignore) {
16815         }
16816         try {
16817             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16818                     gravityNorm, measurements, true,
16819                     new Matrix(1, 3));
16820             fail("IllegalArgumentException expected but not thrown");
16821         } catch (final IllegalArgumentException ignore) {
16822         }
16823         assertNull(calibrator);
16824     }
16825 
16826     @Test
16827     public void testConstructor148() throws WrongSizeException {
16828         final Collection<StandardDeviationBodyKinematics> measurements =
16829                 Collections.emptyList();
16830 
16831         final Matrix ba = generateBa();
16832         final double[] bias = ba.getBuffer();
16833         final double biasX = ba.getElementAtIndex(0);
16834         final double biasY = ba.getElementAtIndex(1);
16835         final double biasZ = ba.getElementAtIndex(2);
16836 
16837         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16838         final double latitude = Math.toRadians(
16839                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16840         final double longitude = Math.toRadians(
16841                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16842         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16843         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16844         final NEDVelocity nedVelocity = new NEDVelocity();
16845         final ECEFPosition ecefPosition = new ECEFPosition();
16846         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16847         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16848                 ecefPosition, ecefVelocity);
16849         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16850                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16851         final double gravityNorm = gravity.getNorm();
16852 
16853         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
16854                 new KnownBiasAndGravityNormAccelerometerCalibrator(
16855                         gravityNorm, measurements,
16856                         true, ba, this);
16857 
16858         // check default values
16859         assertEquals(calibrator.getBiasX(), biasX, 0.0);
16860         assertEquals(calibrator.getBiasY(), biasY, 0.0);
16861         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
16862         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
16863         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16864         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16865         final Acceleration bx2 = new Acceleration(0.0,
16866                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16867         calibrator.getBiasXAsAcceleration(bx2);
16868         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16869         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16870         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
16871         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16872         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16873         final Acceleration by2 = new Acceleration(0.0,
16874                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16875         calibrator.getBiasYAsAcceleration(by2);
16876         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16877         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16878         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
16879         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16880         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16881         final Acceleration bz2 = new Acceleration(0.0,
16882                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16883         calibrator.getBiasZAsAcceleration(bz2);
16884         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16885         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16886         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16887         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16888         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16889         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16890         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16891         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16892         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16893         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16894         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16895         final double[] bias1 = calibrator.getBias();
16896         assertArrayEquals(bias1, bias, 0.0);
16897         final double[] bias2 = new double[3];
16898         calibrator.getBias(bias2);
16899         assertArrayEquals(bias1, bias2, 0.0);
16900         final Matrix b1 = calibrator.getBiasAsMatrix();
16901         assertEquals(b1, ba);
16902         final Matrix b2 = new Matrix(3, 1);
16903         calibrator.getBiasAsMatrix(b2);
16904         assertEquals(b1, b2);
16905         final Matrix ma1 = calibrator.getInitialMa();
16906         assertEquals(ma1, new Matrix(3, 3));
16907         final Matrix ma2 = new Matrix(3, 3);
16908         calibrator.getInitialMa(ma2);
16909         assertEquals(ma1, ma2);
16910         assertSame(calibrator.getMeasurements(), measurements);
16911         assertTrue(calibrator.isCommonAxisUsed());
16912         assertSame(calibrator.getListener(), this);
16913         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
16914         assertFalse(calibrator.isReady());
16915         assertFalse(calibrator.isRunning());
16916         assertNull(calibrator.getEstimatedMa());
16917         assertNull(calibrator.getEstimatedSx());
16918         assertNull(calibrator.getEstimatedSy());
16919         assertNull(calibrator.getEstimatedSz());
16920         assertNull(calibrator.getEstimatedMxy());
16921         assertNull(calibrator.getEstimatedMxz());
16922         assertNull(calibrator.getEstimatedMyx());
16923         assertNull(calibrator.getEstimatedMyz());
16924         assertNull(calibrator.getEstimatedMzx());
16925         assertNull(calibrator.getEstimatedMzy());
16926         assertNull(calibrator.getEstimatedCovariance());
16927         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16928         assertNotNull(calibrator.getGroundTruthGravityNorm());
16929         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16930         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16931         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16932                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16933         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16934         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16935         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16936 
16937         // Force IllegalArgumentException
16938         calibrator = null;
16939         try {
16940             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16941                     -gravityNorm, measurements,
16942                     true, ba, this);
16943             fail("IllegalArgumentException expected but not thrown");
16944         } catch (final IllegalArgumentException ignore) {
16945         }
16946         try {
16947             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16948                     gravityNorm, measurements, true,
16949                     new Matrix(1, 1), this);
16950             fail("IllegalArgumentException expected but not thrown");
16951         } catch (final IllegalArgumentException ignore) {
16952         }
16953         try {
16954             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
16955                     gravityNorm, measurements, true,
16956                     new Matrix(1, 3), this);
16957             fail("IllegalArgumentException expected but not thrown");
16958         } catch (final IllegalArgumentException ignore) {
16959         }
16960         assertNull(calibrator);
16961     }
16962 
16963     @Test
16964     public void testConstructor149() throws WrongSizeException {
16965         final Matrix ba = generateBa();
16966         final double[] bias = ba.getBuffer();
16967         final double biasX = ba.getElementAtIndex(0);
16968         final double biasY = ba.getElementAtIndex(1);
16969         final double biasZ = ba.getElementAtIndex(2);
16970 
16971         final Matrix ma = generateMaCommonAxis();
16972         final double sx = ma.getElementAt(0, 0);
16973         final double sy = ma.getElementAt(1, 1);
16974         final double sz = ma.getElementAt(2, 2);
16975         final double mxy = ma.getElementAt(0, 1);
16976         final double mxz = ma.getElementAt(0, 2);
16977         final double myx = ma.getElementAt(1, 0);
16978         final double myz = ma.getElementAt(1, 2);
16979         final double mzx = ma.getElementAt(2, 0);
16980         final double mzy = ma.getElementAt(2, 1);
16981 
16982         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16983         final double latitude = Math.toRadians(
16984                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16985         final double longitude = Math.toRadians(
16986                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16987         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16988         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16989         final NEDVelocity nedVelocity = new NEDVelocity();
16990         final ECEFPosition ecefPosition = new ECEFPosition();
16991         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16992         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16993                 ecefPosition, ecefVelocity);
16994         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16995                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16996         final double gravityNorm = gravity.getNorm();
16997 
16998         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
16999                 new KnownBiasAndGravityNormAccelerometerCalibrator(
17000                         gravityNorm, ba, ma);
17001 
17002         // check default values
17003         assertEquals(calibrator.getBiasX(), biasX, 0.0);
17004         assertEquals(calibrator.getBiasY(), biasY, 0.0);
17005         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
17006         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
17007         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17008         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17009         final Acceleration bx2 = new Acceleration(0.0,
17010                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17011         calibrator.getBiasXAsAcceleration(bx2);
17012         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17013         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17014         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
17015         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17016         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17017         final Acceleration by2 = new Acceleration(0.0,
17018                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17019         calibrator.getBiasYAsAcceleration(by2);
17020         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17021         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17022         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
17023         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17024         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17025         final Acceleration bz2 = new Acceleration(0.0,
17026                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17027         calibrator.getBiasZAsAcceleration(bz2);
17028         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17029         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17030         assertEquals(calibrator.getInitialSx(), sx, 0.0);
17031         assertEquals(calibrator.getInitialSy(), sy, 0.0);
17032         assertEquals(calibrator.getInitialSz(), sz, 0.0);
17033         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
17034         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
17035         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
17036         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
17037         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
17038         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
17039         final double[] bias1 = calibrator.getBias();
17040         assertArrayEquals(bias1, bias, 0.0);
17041         final double[] bias2 = new double[3];
17042         calibrator.getBias(bias2);
17043         assertArrayEquals(bias1, bias2, 0.0);
17044         final Matrix b1 = calibrator.getBiasAsMatrix();
17045         assertEquals(b1, ba);
17046         final Matrix b2 = new Matrix(3, 1);
17047         calibrator.getBiasAsMatrix(b2);
17048         assertEquals(b1, b2);
17049         final Matrix ma1 = new Matrix(3, 3);
17050         ma1.setSubmatrix(0, 0,
17051                 2, 2,
17052                 new double[]{sx, myx, mzx,
17053                         mxy, sy, mzy,
17054                         mxz, myz, sz});
17055         assertEquals(calibrator.getInitialMa(), ma1);
17056         final Matrix ma2 = new Matrix(3, 3);
17057         calibrator.getInitialMa(ma2);
17058         assertEquals(ma1, ma2);
17059         assertNull(calibrator.getMeasurements());
17060         assertFalse(calibrator.isCommonAxisUsed());
17061         assertNull(calibrator.getListener());
17062         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17063         assertFalse(calibrator.isReady());
17064         assertFalse(calibrator.isRunning());
17065         assertNull(calibrator.getEstimatedMa());
17066         assertNull(calibrator.getEstimatedSx());
17067         assertNull(calibrator.getEstimatedSy());
17068         assertNull(calibrator.getEstimatedSz());
17069         assertNull(calibrator.getEstimatedMxy());
17070         assertNull(calibrator.getEstimatedMxz());
17071         assertNull(calibrator.getEstimatedMyx());
17072         assertNull(calibrator.getEstimatedMyz());
17073         assertNull(calibrator.getEstimatedMzx());
17074         assertNull(calibrator.getEstimatedMzy());
17075         assertNull(calibrator.getEstimatedCovariance());
17076         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17077         assertNotNull(calibrator.getGroundTruthGravityNorm());
17078         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17079         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17080         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17081                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17082         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17083         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17084         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17085 
17086         // Force IllegalArgumentException
17087         calibrator = null;
17088         try {
17089             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17090                     -gravityNorm, ba, ma);
17091             fail("IllegalArgumentException expected but not thrown");
17092         } catch (final IllegalArgumentException ignore) {
17093         }
17094         try {
17095             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17096                     gravityNorm, new Matrix(1, 1), ma);
17097             fail("IllegalArgumentException expected but not thrown");
17098         } catch (final IllegalArgumentException ignore) {
17099         }
17100         try {
17101             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17102                     gravityNorm, new Matrix(1, 3), ma);
17103             fail("IllegalArgumentException expected but not thrown");
17104         } catch (final IllegalArgumentException ignore) {
17105         }
17106         try {
17107             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17108                     gravityNorm, ba, new Matrix(1, 3));
17109             fail("IllegalArgumentException expected but not thrown");
17110         } catch (final IllegalArgumentException ignore) {
17111         }
17112         try {
17113             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17114                     gravityNorm, ba, new Matrix(3, 1));
17115             fail("IllegalArgumentException expected but not thrown");
17116         } catch (final IllegalArgumentException ignore) {
17117         }
17118         assertNull(calibrator);
17119     }
17120 
17121     @Test
17122     public void testConstructor150() throws WrongSizeException {
17123         final Matrix ba = generateBa();
17124         final double[] bias = ba.getBuffer();
17125         final double biasX = ba.getElementAtIndex(0);
17126         final double biasY = ba.getElementAtIndex(1);
17127         final double biasZ = ba.getElementAtIndex(2);
17128 
17129         final Matrix ma = generateMaCommonAxis();
17130         final double sx = ma.getElementAt(0, 0);
17131         final double sy = ma.getElementAt(1, 1);
17132         final double sz = ma.getElementAt(2, 2);
17133         final double mxy = ma.getElementAt(0, 1);
17134         final double mxz = ma.getElementAt(0, 2);
17135         final double myx = ma.getElementAt(1, 0);
17136         final double myz = ma.getElementAt(1, 2);
17137         final double mzx = ma.getElementAt(2, 0);
17138         final double mzy = ma.getElementAt(2, 1);
17139 
17140         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17141         final double latitude = Math.toRadians(
17142                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17143         final double longitude = Math.toRadians(
17144                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17145         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17146         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17147         final NEDVelocity nedVelocity = new NEDVelocity();
17148         final ECEFPosition ecefPosition = new ECEFPosition();
17149         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17150         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17151                 ecefPosition, ecefVelocity);
17152         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17153                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17154         final double gravityNorm = gravity.getNorm();
17155 
17156         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
17157                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
17158                         ba, ma, this);
17159 
17160         // check default values
17161         assertEquals(calibrator.getBiasX(), biasX, 0.0);
17162         assertEquals(calibrator.getBiasY(), biasY, 0.0);
17163         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
17164         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
17165         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17166         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17167         final Acceleration bx2 = new Acceleration(0.0,
17168                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17169         calibrator.getBiasXAsAcceleration(bx2);
17170         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17171         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17172         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
17173         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17174         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17175         final Acceleration by2 = new Acceleration(0.0,
17176                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17177         calibrator.getBiasYAsAcceleration(by2);
17178         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17179         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17180         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
17181         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17182         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17183         final Acceleration bz2 = new Acceleration(0.0,
17184                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17185         calibrator.getBiasZAsAcceleration(bz2);
17186         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17187         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17188         assertEquals(calibrator.getInitialSx(), sx, 0.0);
17189         assertEquals(calibrator.getInitialSy(), sy, 0.0);
17190         assertEquals(calibrator.getInitialSz(), sz, 0.0);
17191         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
17192         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
17193         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
17194         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
17195         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
17196         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
17197         final double[] bias1 = calibrator.getBias();
17198         assertArrayEquals(bias1, bias, 0.0);
17199         final double[] bias2 = new double[3];
17200         calibrator.getBias(bias2);
17201         assertArrayEquals(bias1, bias2, 0.0);
17202         final Matrix b1 = calibrator.getBiasAsMatrix();
17203         assertEquals(b1, ba);
17204         final Matrix b2 = new Matrix(3, 1);
17205         calibrator.getBiasAsMatrix(b2);
17206         assertEquals(b1, b2);
17207         final Matrix ma1 = new Matrix(3, 3);
17208         ma1.setSubmatrix(0, 0,
17209                 2, 2,
17210                 new double[]{sx, myx, mzx,
17211                         mxy, sy, mzy,
17212                         mxz, myz, sz});
17213         assertEquals(calibrator.getInitialMa(), ma1);
17214         final Matrix ma2 = new Matrix(3, 3);
17215         calibrator.getInitialMa(ma2);
17216         assertEquals(ma1, ma2);
17217         assertNull(calibrator.getMeasurements());
17218         assertFalse(calibrator.isCommonAxisUsed());
17219         assertSame(calibrator.getListener(), this);
17220         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17221         assertFalse(calibrator.isReady());
17222         assertFalse(calibrator.isRunning());
17223         assertNull(calibrator.getEstimatedMa());
17224         assertNull(calibrator.getEstimatedSx());
17225         assertNull(calibrator.getEstimatedSy());
17226         assertNull(calibrator.getEstimatedSz());
17227         assertNull(calibrator.getEstimatedMxy());
17228         assertNull(calibrator.getEstimatedMxz());
17229         assertNull(calibrator.getEstimatedMyx());
17230         assertNull(calibrator.getEstimatedMyz());
17231         assertNull(calibrator.getEstimatedMzx());
17232         assertNull(calibrator.getEstimatedMzy());
17233         assertNull(calibrator.getEstimatedCovariance());
17234         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17235         assertNotNull(calibrator.getGroundTruthGravityNorm());
17236         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17237         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17238         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17239                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17240         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17241         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17242         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17243 
17244         // Force IllegalArgumentException
17245         calibrator = null;
17246         try {
17247             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17248                     -gravityNorm, ba, ma, this);
17249             fail("IllegalArgumentException expected but not thrown");
17250         } catch (final IllegalArgumentException ignore) {
17251         }
17252         try {
17253             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17254                     gravityNorm, new Matrix(1, 1), ma,
17255                     this);
17256             fail("IllegalArgumentException expected but not thrown");
17257         } catch (final IllegalArgumentException ignore) {
17258         }
17259         try {
17260             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17261                     gravityNorm, new Matrix(1, 3), ma,
17262                     this);
17263             fail("IllegalArgumentException expected but not thrown");
17264         } catch (final IllegalArgumentException ignore) {
17265         }
17266         try {
17267             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17268                     gravityNorm, ba, new Matrix(1, 3), this);
17269             fail("IllegalArgumentException expected but not thrown");
17270         } catch (final IllegalArgumentException ignore) {
17271         }
17272         try {
17273             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17274                     gravityNorm, ba, new Matrix(3, 1), this);
17275             fail("IllegalArgumentException expected but not thrown");
17276         } catch (final IllegalArgumentException ignore) {
17277         }
17278         assertNull(calibrator);
17279     }
17280 
17281     @Test
17282     public void testConstructor151() throws WrongSizeException {
17283         final Collection<StandardDeviationBodyKinematics> measurements =
17284                 Collections.emptyList();
17285 
17286         final Matrix ba = generateBa();
17287         final double[] bias = ba.getBuffer();
17288         final double biasX = ba.getElementAtIndex(0);
17289         final double biasY = ba.getElementAtIndex(1);
17290         final double biasZ = ba.getElementAtIndex(2);
17291 
17292         final Matrix ma = generateMaCommonAxis();
17293         final double sx = ma.getElementAt(0, 0);
17294         final double sy = ma.getElementAt(1, 1);
17295         final double sz = ma.getElementAt(2, 2);
17296         final double mxy = ma.getElementAt(0, 1);
17297         final double mxz = ma.getElementAt(0, 2);
17298         final double myx = ma.getElementAt(1, 0);
17299         final double myz = ma.getElementAt(1, 2);
17300         final double mzx = ma.getElementAt(2, 0);
17301         final double mzy = ma.getElementAt(2, 1);
17302 
17303         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17304         final double latitude = Math.toRadians(
17305                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17306         final double longitude = Math.toRadians(
17307                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17308         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17309         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17310         final NEDVelocity nedVelocity = new NEDVelocity();
17311         final ECEFPosition ecefPosition = new ECEFPosition();
17312         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17313         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17314                 ecefPosition, ecefVelocity);
17315         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17316                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17317         final double gravityNorm = gravity.getNorm();
17318 
17319         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
17320                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
17321                         measurements, ba, ma);
17322 
17323         // check default values
17324         assertEquals(calibrator.getBiasX(), biasX, 0.0);
17325         assertEquals(calibrator.getBiasY(), biasY, 0.0);
17326         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
17327         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
17328         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17329         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17330         final Acceleration bx2 = new Acceleration(0.0,
17331                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17332         calibrator.getBiasXAsAcceleration(bx2);
17333         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17334         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17335         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
17336         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17337         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17338         final Acceleration by2 = new Acceleration(0.0,
17339                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17340         calibrator.getBiasYAsAcceleration(by2);
17341         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17342         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17343         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
17344         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17345         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17346         final Acceleration bz2 = new Acceleration(0.0,
17347                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17348         calibrator.getBiasZAsAcceleration(bz2);
17349         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17350         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17351         assertEquals(calibrator.getInitialSx(), sx, 0.0);
17352         assertEquals(calibrator.getInitialSy(), sy, 0.0);
17353         assertEquals(calibrator.getInitialSz(), sz, 0.0);
17354         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
17355         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
17356         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
17357         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
17358         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
17359         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
17360         final double[] bias1 = calibrator.getBias();
17361         assertArrayEquals(bias1, bias, 0.0);
17362         final double[] bias2 = new double[3];
17363         calibrator.getBias(bias2);
17364         assertArrayEquals(bias1, bias2, 0.0);
17365         final Matrix b1 = calibrator.getBiasAsMatrix();
17366         assertEquals(b1, ba);
17367         final Matrix b2 = new Matrix(3, 1);
17368         calibrator.getBiasAsMatrix(b2);
17369         assertEquals(b1, b2);
17370         final Matrix ma1 = new Matrix(3, 3);
17371         ma1.setSubmatrix(0, 0,
17372                 2, 2,
17373                 new double[]{sx, myx, mzx,
17374                         mxy, sy, mzy,
17375                         mxz, myz, sz});
17376         assertEquals(calibrator.getInitialMa(), ma1);
17377         final Matrix ma2 = new Matrix(3, 3);
17378         calibrator.getInitialMa(ma2);
17379         assertEquals(ma1, ma2);
17380         assertSame(calibrator.getMeasurements(), measurements);
17381         assertFalse(calibrator.isCommonAxisUsed());
17382         assertNull(calibrator.getListener());
17383         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17384         assertFalse(calibrator.isReady());
17385         assertFalse(calibrator.isRunning());
17386         assertNull(calibrator.getEstimatedMa());
17387         assertNull(calibrator.getEstimatedSx());
17388         assertNull(calibrator.getEstimatedSy());
17389         assertNull(calibrator.getEstimatedSz());
17390         assertNull(calibrator.getEstimatedMxy());
17391         assertNull(calibrator.getEstimatedMxz());
17392         assertNull(calibrator.getEstimatedMyx());
17393         assertNull(calibrator.getEstimatedMyz());
17394         assertNull(calibrator.getEstimatedMzx());
17395         assertNull(calibrator.getEstimatedMzy());
17396         assertNull(calibrator.getEstimatedCovariance());
17397         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17398         assertNotNull(calibrator.getGroundTruthGravityNorm());
17399         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17400         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17401         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17402                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17403         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17404         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17405         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17406 
17407         // Force IllegalArgumentException
17408         calibrator = null;
17409         try {
17410             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17411                     -gravityNorm, measurements, ba, ma);
17412             fail("IllegalArgumentException expected but not thrown");
17413         } catch (final IllegalArgumentException ignore) {
17414         }
17415         try {
17416             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17417                     gravityNorm, measurements,
17418                     new Matrix(1, 1), ma);
17419             fail("IllegalArgumentException expected but not thrown");
17420         } catch (final IllegalArgumentException ignore) {
17421         }
17422         try {
17423             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17424                     gravityNorm, measurements,
17425                     new Matrix(1, 3), ma);
17426             fail("IllegalArgumentException expected but not thrown");
17427         } catch (final IllegalArgumentException ignore) {
17428         }
17429         try {
17430             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17431                     gravityNorm, measurements, ba,
17432                     new Matrix(1, 3));
17433             fail("IllegalArgumentException expected but not thrown");
17434         } catch (final IllegalArgumentException ignore) {
17435         }
17436         try {
17437             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17438                     gravityNorm, measurements, ba,
17439                     new Matrix(3, 1));
17440             fail("IllegalArgumentException expected but not thrown");
17441         } catch (final IllegalArgumentException ignore) {
17442         }
17443         assertNull(calibrator);
17444     }
17445 
17446     @Test
17447     public void testConstructor152() throws WrongSizeException {
17448         final Collection<StandardDeviationBodyKinematics> measurements =
17449                 Collections.emptyList();
17450 
17451         final Matrix ba = generateBa();
17452         final double[] bias = ba.getBuffer();
17453         final double biasX = ba.getElementAtIndex(0);
17454         final double biasY = ba.getElementAtIndex(1);
17455         final double biasZ = ba.getElementAtIndex(2);
17456 
17457         final Matrix ma = generateMaCommonAxis();
17458         final double sx = ma.getElementAt(0, 0);
17459         final double sy = ma.getElementAt(1, 1);
17460         final double sz = ma.getElementAt(2, 2);
17461         final double mxy = ma.getElementAt(0, 1);
17462         final double mxz = ma.getElementAt(0, 2);
17463         final double myx = ma.getElementAt(1, 0);
17464         final double myz = ma.getElementAt(1, 2);
17465         final double mzx = ma.getElementAt(2, 0);
17466         final double mzy = ma.getElementAt(2, 1);
17467 
17468         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17469         final double latitude = Math.toRadians(
17470                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17471         final double longitude = Math.toRadians(
17472                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17473         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17474         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17475         final NEDVelocity nedVelocity = new NEDVelocity();
17476         final ECEFPosition ecefPosition = new ECEFPosition();
17477         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17478         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17479                 ecefPosition, ecefVelocity);
17480         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17481                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17482         final double gravityNorm = gravity.getNorm();
17483 
17484         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
17485                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
17486                         measurements, ba, ma, this);
17487 
17488         // check default values
17489         assertEquals(calibrator.getBiasX(), biasX, 0.0);
17490         assertEquals(calibrator.getBiasY(), biasY, 0.0);
17491         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
17492         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
17493         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17494         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17495         final Acceleration bx2 = new Acceleration(0.0,
17496                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17497         calibrator.getBiasXAsAcceleration(bx2);
17498         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17499         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17500         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
17501         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17502         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17503         final Acceleration by2 = new Acceleration(0.0,
17504                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17505         calibrator.getBiasYAsAcceleration(by2);
17506         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17507         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17508         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
17509         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17510         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17511         final Acceleration bz2 = new Acceleration(0.0,
17512                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17513         calibrator.getBiasZAsAcceleration(bz2);
17514         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17515         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17516         assertEquals(calibrator.getInitialSx(), sx, 0.0);
17517         assertEquals(calibrator.getInitialSy(), sy, 0.0);
17518         assertEquals(calibrator.getInitialSz(), sz, 0.0);
17519         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
17520         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
17521         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
17522         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
17523         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
17524         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
17525         final double[] bias1 = calibrator.getBias();
17526         assertArrayEquals(bias1, bias, 0.0);
17527         final double[] bias2 = new double[3];
17528         calibrator.getBias(bias2);
17529         assertArrayEquals(bias1, bias2, 0.0);
17530         final Matrix b1 = calibrator.getBiasAsMatrix();
17531         assertEquals(b1, ba);
17532         final Matrix b2 = new Matrix(3, 1);
17533         calibrator.getBiasAsMatrix(b2);
17534         assertEquals(b1, b2);
17535         final Matrix ma1 = new Matrix(3, 3);
17536         ma1.setSubmatrix(0, 0,
17537                 2, 2,
17538                 new double[]{sx, myx, mzx,
17539                         mxy, sy, mzy,
17540                         mxz, myz, sz});
17541         assertEquals(calibrator.getInitialMa(), ma1);
17542         final Matrix ma2 = new Matrix(3, 3);
17543         calibrator.getInitialMa(ma2);
17544         assertEquals(ma1, ma2);
17545         assertSame(calibrator.getMeasurements(), measurements);
17546         assertFalse(calibrator.isCommonAxisUsed());
17547         assertSame(calibrator.getListener(), this);
17548         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17549         assertFalse(calibrator.isReady());
17550         assertFalse(calibrator.isRunning());
17551         assertNull(calibrator.getEstimatedMa());
17552         assertNull(calibrator.getEstimatedSx());
17553         assertNull(calibrator.getEstimatedSy());
17554         assertNull(calibrator.getEstimatedSz());
17555         assertNull(calibrator.getEstimatedMxy());
17556         assertNull(calibrator.getEstimatedMxz());
17557         assertNull(calibrator.getEstimatedMyx());
17558         assertNull(calibrator.getEstimatedMyz());
17559         assertNull(calibrator.getEstimatedMzx());
17560         assertNull(calibrator.getEstimatedMzy());
17561         assertNull(calibrator.getEstimatedCovariance());
17562         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17563         assertNotNull(calibrator.getGroundTruthGravityNorm());
17564         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17565         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17566         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17567                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17568         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17569         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17570         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17571 
17572         // Force IllegalArgumentException
17573         calibrator = null;
17574         try {
17575             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17576                     -gravityNorm, measurements, ba, ma, this);
17577             fail("IllegalArgumentException expected but not thrown");
17578         } catch (final IllegalArgumentException ignore) {
17579         }
17580         try {
17581             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17582                     gravityNorm, measurements,
17583                     new Matrix(1, 1), ma, this);
17584             fail("IllegalArgumentException expected but not thrown");
17585         } catch (final IllegalArgumentException ignore) {
17586         }
17587         try {
17588             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17589                     gravityNorm, measurements,
17590                     new Matrix(1, 3), ma, this);
17591             fail("IllegalArgumentException expected but not thrown");
17592         } catch (final IllegalArgumentException ignore) {
17593         }
17594         try {
17595             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17596                     gravityNorm, measurements, ba,
17597                     new Matrix(1, 3), this);
17598             fail("IllegalArgumentException expected but not thrown");
17599         } catch (final IllegalArgumentException ignore) {
17600         }
17601         try {
17602             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17603                     gravityNorm, measurements, ba,
17604                     new Matrix(3, 1), this);
17605             fail("IllegalArgumentException expected but not thrown");
17606         } catch (final IllegalArgumentException ignore) {
17607         }
17608         assertNull(calibrator);
17609     }
17610 
17611     @Test
17612     public void testConstructor153() throws WrongSizeException {
17613         final Matrix ba = generateBa();
17614         final double[] bias = ba.getBuffer();
17615         final double biasX = ba.getElementAtIndex(0);
17616         final double biasY = ba.getElementAtIndex(1);
17617         final double biasZ = ba.getElementAtIndex(2);
17618 
17619         final Matrix ma = generateMaCommonAxis();
17620         final double sx = ma.getElementAt(0, 0);
17621         final double sy = ma.getElementAt(1, 1);
17622         final double sz = ma.getElementAt(2, 2);
17623         final double mxy = ma.getElementAt(0, 1);
17624         final double mxz = ma.getElementAt(0, 2);
17625         final double myx = ma.getElementAt(1, 0);
17626         final double myz = ma.getElementAt(1, 2);
17627         final double mzx = ma.getElementAt(2, 0);
17628         final double mzy = ma.getElementAt(2, 1);
17629 
17630         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17631         final double latitude = Math.toRadians(
17632                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17633         final double longitude = Math.toRadians(
17634                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17635         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17636         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17637         final NEDVelocity nedVelocity = new NEDVelocity();
17638         final ECEFPosition ecefPosition = new ECEFPosition();
17639         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17640         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17641                 ecefPosition, ecefVelocity);
17642         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17643                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17644         final double gravityNorm = gravity.getNorm();
17645 
17646         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
17647                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
17648                         true, ba, ma);
17649 
17650         // check default values
17651         assertEquals(calibrator.getBiasX(), biasX, 0.0);
17652         assertEquals(calibrator.getBiasY(), biasY, 0.0);
17653         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
17654         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
17655         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17656         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17657         final Acceleration bx2 = new Acceleration(0.0,
17658                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17659         calibrator.getBiasXAsAcceleration(bx2);
17660         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17661         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17662         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
17663         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17664         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17665         final Acceleration by2 = new Acceleration(0.0,
17666                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17667         calibrator.getBiasYAsAcceleration(by2);
17668         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17669         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17670         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
17671         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17672         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17673         final Acceleration bz2 = new Acceleration(0.0,
17674                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17675         calibrator.getBiasZAsAcceleration(bz2);
17676         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17677         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17678         assertEquals(calibrator.getInitialSx(), sx, 0.0);
17679         assertEquals(calibrator.getInitialSy(), sy, 0.0);
17680         assertEquals(calibrator.getInitialSz(), sz, 0.0);
17681         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
17682         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
17683         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
17684         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
17685         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
17686         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
17687         final double[] bias1 = calibrator.getBias();
17688         assertArrayEquals(bias1, bias, 0.0);
17689         final double[] bias2 = new double[3];
17690         calibrator.getBias(bias2);
17691         assertArrayEquals(bias1, bias2, 0.0);
17692         final Matrix b1 = calibrator.getBiasAsMatrix();
17693         assertEquals(b1, ba);
17694         final Matrix b2 = new Matrix(3, 1);
17695         calibrator.getBiasAsMatrix(b2);
17696         assertEquals(b1, b2);
17697         final Matrix ma1 = new Matrix(3, 3);
17698         ma1.setSubmatrix(0, 0,
17699                 2, 2,
17700                 new double[]{sx, myx, mzx,
17701                         mxy, sy, mzy,
17702                         mxz, myz, sz});
17703         assertEquals(calibrator.getInitialMa(), ma1);
17704         final Matrix ma2 = new Matrix(3, 3);
17705         calibrator.getInitialMa(ma2);
17706         assertEquals(ma1, ma2);
17707         assertNull(calibrator.getMeasurements());
17708         assertTrue(calibrator.isCommonAxisUsed());
17709         assertNull(calibrator.getListener());
17710         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
17711         assertFalse(calibrator.isReady());
17712         assertFalse(calibrator.isRunning());
17713         assertNull(calibrator.getEstimatedMa());
17714         assertNull(calibrator.getEstimatedSx());
17715         assertNull(calibrator.getEstimatedSy());
17716         assertNull(calibrator.getEstimatedSz());
17717         assertNull(calibrator.getEstimatedMxy());
17718         assertNull(calibrator.getEstimatedMxz());
17719         assertNull(calibrator.getEstimatedMyx());
17720         assertNull(calibrator.getEstimatedMyz());
17721         assertNull(calibrator.getEstimatedMzx());
17722         assertNull(calibrator.getEstimatedMzy());
17723         assertNull(calibrator.getEstimatedCovariance());
17724         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17725         assertNotNull(calibrator.getGroundTruthGravityNorm());
17726         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17727         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17728         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17729                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17730         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17731         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17732         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17733 
17734         // Force IllegalArgumentException
17735         calibrator = null;
17736         try {
17737             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17738                     -gravityNorm, true, ba, ma);
17739             fail("IllegalArgumentException expected but not thrown");
17740         } catch (final IllegalArgumentException ignore) {
17741         }
17742         try {
17743             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17744                     gravityNorm, true,
17745                     new Matrix(1, 1), ma);
17746             fail("IllegalArgumentException expected but not thrown");
17747         } catch (final IllegalArgumentException ignore) {
17748         }
17749         try {
17750             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17751                     gravityNorm, true,
17752                     new Matrix(1, 3), ma);
17753             fail("IllegalArgumentException expected but not thrown");
17754         } catch (final IllegalArgumentException ignore) {
17755         }
17756         try {
17757             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17758                     gravityNorm, true, ba,
17759                     new Matrix(1, 3));
17760             fail("IllegalArgumentException expected but not thrown");
17761         } catch (final IllegalArgumentException ignore) {
17762         }
17763         try {
17764             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17765                     gravityNorm, true, ba,
17766                     new Matrix(3, 1));
17767             fail("IllegalArgumentException expected but not thrown");
17768         } catch (final IllegalArgumentException ignore) {
17769         }
17770         assertNull(calibrator);
17771     }
17772 
17773     @Test
17774     public void testConstructor154() throws WrongSizeException {
17775         final Matrix ba = generateBa();
17776         final double[] bias = ba.getBuffer();
17777         final double biasX = ba.getElementAtIndex(0);
17778         final double biasY = ba.getElementAtIndex(1);
17779         final double biasZ = ba.getElementAtIndex(2);
17780 
17781         final Matrix ma = generateMaCommonAxis();
17782         final double sx = ma.getElementAt(0, 0);
17783         final double sy = ma.getElementAt(1, 1);
17784         final double sz = ma.getElementAt(2, 2);
17785         final double mxy = ma.getElementAt(0, 1);
17786         final double mxz = ma.getElementAt(0, 2);
17787         final double myx = ma.getElementAt(1, 0);
17788         final double myz = ma.getElementAt(1, 2);
17789         final double mzx = ma.getElementAt(2, 0);
17790         final double mzy = ma.getElementAt(2, 1);
17791 
17792         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17793         final double latitude = Math.toRadians(
17794                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17795         final double longitude = Math.toRadians(
17796                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17797         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17798         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17799         final NEDVelocity nedVelocity = new NEDVelocity();
17800         final ECEFPosition ecefPosition = new ECEFPosition();
17801         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17802         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17803                 ecefPosition, ecefVelocity);
17804         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17805                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17806         final double gravityNorm = gravity.getNorm();
17807 
17808         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
17809                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
17810                         true, ba, ma, this);
17811 
17812         // check default values
17813         assertEquals(calibrator.getBiasX(), biasX, 0.0);
17814         assertEquals(calibrator.getBiasY(), biasY, 0.0);
17815         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
17816         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
17817         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17818         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17819         final Acceleration bx2 = new Acceleration(0.0,
17820                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17821         calibrator.getBiasXAsAcceleration(bx2);
17822         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17823         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17824         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
17825         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17826         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17827         final Acceleration by2 = new Acceleration(0.0,
17828                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17829         calibrator.getBiasYAsAcceleration(by2);
17830         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17831         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17832         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
17833         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17834         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17835         final Acceleration bz2 = new Acceleration(0.0,
17836                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17837         calibrator.getBiasZAsAcceleration(bz2);
17838         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17839         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17840         assertEquals(calibrator.getInitialSx(), sx, 0.0);
17841         assertEquals(calibrator.getInitialSy(), sy, 0.0);
17842         assertEquals(calibrator.getInitialSz(), sz, 0.0);
17843         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
17844         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
17845         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
17846         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
17847         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
17848         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
17849         final double[] bias1 = calibrator.getBias();
17850         assertArrayEquals(bias1, bias, 0.0);
17851         final double[] bias2 = new double[3];
17852         calibrator.getBias(bias2);
17853         assertArrayEquals(bias1, bias2, 0.0);
17854         final Matrix b1 = calibrator.getBiasAsMatrix();
17855         assertEquals(b1, ba);
17856         final Matrix b2 = new Matrix(3, 1);
17857         calibrator.getBiasAsMatrix(b2);
17858         assertEquals(b1, b2);
17859         final Matrix ma1 = new Matrix(3, 3);
17860         ma1.setSubmatrix(0, 0,
17861                 2, 2,
17862                 new double[]{sx, myx, mzx,
17863                         mxy, sy, mzy,
17864                         mxz, myz, sz});
17865         assertEquals(calibrator.getInitialMa(), ma1);
17866         final Matrix ma2 = new Matrix(3, 3);
17867         calibrator.getInitialMa(ma2);
17868         assertEquals(ma1, ma2);
17869         assertNull(calibrator.getMeasurements());
17870         assertTrue(calibrator.isCommonAxisUsed());
17871         assertSame(calibrator.getListener(), this);
17872         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
17873         assertFalse(calibrator.isReady());
17874         assertFalse(calibrator.isRunning());
17875         assertNull(calibrator.getEstimatedMa());
17876         assertNull(calibrator.getEstimatedSx());
17877         assertNull(calibrator.getEstimatedSy());
17878         assertNull(calibrator.getEstimatedSz());
17879         assertNull(calibrator.getEstimatedMxy());
17880         assertNull(calibrator.getEstimatedMxz());
17881         assertNull(calibrator.getEstimatedMyx());
17882         assertNull(calibrator.getEstimatedMyz());
17883         assertNull(calibrator.getEstimatedMzx());
17884         assertNull(calibrator.getEstimatedMzy());
17885         assertNull(calibrator.getEstimatedCovariance());
17886         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17887         assertNotNull(calibrator.getGroundTruthGravityNorm());
17888         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17889         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17890         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17891                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17892         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17893         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17894         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17895 
17896         // Force IllegalArgumentException
17897         calibrator = null;
17898         try {
17899             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17900                     -gravityNorm, true, ba, ma, this);
17901             fail("IllegalArgumentException expected but not thrown");
17902         } catch (final IllegalArgumentException ignore) {
17903         }
17904         try {
17905             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17906                     gravityNorm, true, new Matrix(1, 1),
17907                     ma, this);
17908             fail("IllegalArgumentException expected but not thrown");
17909         } catch (final IllegalArgumentException ignore) {
17910         }
17911         try {
17912             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17913                     gravityNorm, true, new Matrix(1, 3),
17914                     ma, this);
17915             fail("IllegalArgumentException expected but not thrown");
17916         } catch (final IllegalArgumentException ignore) {
17917         }
17918         try {
17919             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17920                     gravityNorm, true, ba, new Matrix(1, 3),
17921                     this);
17922             fail("IllegalArgumentException expected but not thrown");
17923         } catch (final IllegalArgumentException ignore) {
17924         }
17925         try {
17926             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17927                     gravityNorm, true, ba, new Matrix(3, 1),
17928                     this);
17929             fail("IllegalArgumentException expected but not thrown");
17930         } catch (final IllegalArgumentException ignore) {
17931         }
17932         assertNull(calibrator);
17933     }
17934 
17935     @Test
17936     public void testConstructor155() throws WrongSizeException {
17937         final Collection<StandardDeviationBodyKinematics> measurements =
17938                 Collections.emptyList();
17939 
17940         final Matrix ba = generateBa();
17941         final double[] bias = ba.getBuffer();
17942         final double biasX = ba.getElementAtIndex(0);
17943         final double biasY = ba.getElementAtIndex(1);
17944         final double biasZ = ba.getElementAtIndex(2);
17945 
17946         final Matrix ma = generateMaCommonAxis();
17947         final double sx = ma.getElementAt(0, 0);
17948         final double sy = ma.getElementAt(1, 1);
17949         final double sz = ma.getElementAt(2, 2);
17950         final double mxy = ma.getElementAt(0, 1);
17951         final double mxz = ma.getElementAt(0, 2);
17952         final double myx = ma.getElementAt(1, 0);
17953         final double myz = ma.getElementAt(1, 2);
17954         final double mzx = ma.getElementAt(2, 0);
17955         final double mzy = ma.getElementAt(2, 1);
17956 
17957         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17958         final double latitude = Math.toRadians(
17959                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17960         final double longitude = Math.toRadians(
17961                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17962         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17963         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17964         final NEDVelocity nedVelocity = new NEDVelocity();
17965         final ECEFPosition ecefPosition = new ECEFPosition();
17966         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17967         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17968                 ecefPosition, ecefVelocity);
17969         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17970                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17971         final double gravityNorm = gravity.getNorm();
17972 
17973         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
17974                 new KnownBiasAndGravityNormAccelerometerCalibrator(
17975                         gravityNorm, measurements,
17976                         true, ba, ma);
17977 
17978         // check default values
17979         assertEquals(calibrator.getBiasX(), biasX, 0.0);
17980         assertEquals(calibrator.getBiasY(), biasY, 0.0);
17981         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
17982         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
17983         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17984         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17985         final Acceleration bx2 = new Acceleration(0.0,
17986                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17987         calibrator.getBiasXAsAcceleration(bx2);
17988         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17989         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17990         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
17991         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17992         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17993         final Acceleration by2 = new Acceleration(0.0,
17994                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17995         calibrator.getBiasYAsAcceleration(by2);
17996         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17997         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17998         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
17999         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18000         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18001         final Acceleration bz2 = new Acceleration(0.0,
18002                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18003         calibrator.getBiasZAsAcceleration(bz2);
18004         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18005         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18006         assertEquals(calibrator.getInitialSx(), sx, 0.0);
18007         assertEquals(calibrator.getInitialSy(), sy, 0.0);
18008         assertEquals(calibrator.getInitialSz(), sz, 0.0);
18009         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18010         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18011         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18012         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18013         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18014         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18015         final double[] bias1 = calibrator.getBias();
18016         assertArrayEquals(bias1, bias, 0.0);
18017         final double[] bias2 = new double[3];
18018         calibrator.getBias(bias2);
18019         assertArrayEquals(bias1, bias2, 0.0);
18020         final Matrix b1 = calibrator.getBiasAsMatrix();
18021         assertEquals(b1, ba);
18022         final Matrix b2 = new Matrix(3, 1);
18023         calibrator.getBiasAsMatrix(b2);
18024         assertEquals(b1, b2);
18025         final Matrix ma1 = new Matrix(3, 3);
18026         ma1.setSubmatrix(0, 0,
18027                 2, 2,
18028                 new double[]{sx, myx, mzx,
18029                         mxy, sy, mzy,
18030                         mxz, myz, sz});
18031         assertEquals(calibrator.getInitialMa(), ma1);
18032         final Matrix ma2 = new Matrix(3, 3);
18033         calibrator.getInitialMa(ma2);
18034         assertEquals(ma1, ma2);
18035         assertSame(calibrator.getMeasurements(), measurements);
18036         assertTrue(calibrator.isCommonAxisUsed());
18037         assertNull(calibrator.getListener());
18038         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
18039         assertFalse(calibrator.isReady());
18040         assertFalse(calibrator.isRunning());
18041         assertNull(calibrator.getEstimatedMa());
18042         assertNull(calibrator.getEstimatedSx());
18043         assertNull(calibrator.getEstimatedSy());
18044         assertNull(calibrator.getEstimatedSz());
18045         assertNull(calibrator.getEstimatedMxy());
18046         assertNull(calibrator.getEstimatedMxz());
18047         assertNull(calibrator.getEstimatedMyx());
18048         assertNull(calibrator.getEstimatedMyz());
18049         assertNull(calibrator.getEstimatedMzx());
18050         assertNull(calibrator.getEstimatedMzy());
18051         assertNull(calibrator.getEstimatedCovariance());
18052         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18053         assertNotNull(calibrator.getGroundTruthGravityNorm());
18054         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18055         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18056         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18057                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18058         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18059         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18060         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18061 
18062         // Force IllegalArgumentException
18063         calibrator = null;
18064         try {
18065             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18066                     -gravityNorm, measurements,
18067                     true, ba, ma);
18068             fail("IllegalArgumentException expected but not thrown");
18069         } catch (final IllegalArgumentException ignore) {
18070         }
18071         try {
18072             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18073                     gravityNorm, measurements, true,
18074                     new Matrix(1, 1), ma);
18075             fail("IllegalArgumentException expected but not thrown");
18076         } catch (final IllegalArgumentException ignore) {
18077         }
18078         try {
18079             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18080                     gravityNorm, measurements, true,
18081                     new Matrix(1, 3), ma);
18082             fail("IllegalArgumentException expected but not thrown");
18083         } catch (final IllegalArgumentException ignore) {
18084         }
18085         try {
18086             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18087                     gravityNorm, measurements, true,
18088                     ba, new Matrix(1, 3));
18089             fail("IllegalArgumentException expected but not thrown");
18090         } catch (final IllegalArgumentException ignore) {
18091         }
18092         try {
18093             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18094                     gravityNorm, measurements, true,
18095                     ba, new Matrix(3, 1));
18096             fail("IllegalArgumentException expected but not thrown");
18097         } catch (final IllegalArgumentException ignore) {
18098         }
18099         assertNull(calibrator);
18100     }
18101 
18102     @Test
18103     public void testConstructor156() throws WrongSizeException {
18104         final Collection<StandardDeviationBodyKinematics> measurements =
18105                 Collections.emptyList();
18106 
18107         final Matrix ba = generateBa();
18108         final double[] bias = ba.getBuffer();
18109         final double biasX = ba.getElementAtIndex(0);
18110         final double biasY = ba.getElementAtIndex(1);
18111         final double biasZ = ba.getElementAtIndex(2);
18112 
18113         final Matrix ma = generateMaCommonAxis();
18114         final double sx = ma.getElementAt(0, 0);
18115         final double sy = ma.getElementAt(1, 1);
18116         final double sz = ma.getElementAt(2, 2);
18117         final double mxy = ma.getElementAt(0, 1);
18118         final double mxz = ma.getElementAt(0, 2);
18119         final double myx = ma.getElementAt(1, 0);
18120         final double myz = ma.getElementAt(1, 2);
18121         final double mzx = ma.getElementAt(2, 0);
18122         final double mzy = ma.getElementAt(2, 1);
18123 
18124         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18125         final double latitude = Math.toRadians(
18126                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18127         final double longitude = Math.toRadians(
18128                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18129         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18130         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18131         final NEDVelocity nedVelocity = new NEDVelocity();
18132         final ECEFPosition ecefPosition = new ECEFPosition();
18133         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18134         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18135                 ecefPosition, ecefVelocity);
18136         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18137                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18138         final double gravityNorm = gravity.getNorm();
18139 
18140         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
18141                 new KnownBiasAndGravityNormAccelerometerCalibrator(
18142                         gravityNorm, measurements,
18143                         true, ba, ma, this);
18144 
18145         // check default values
18146         assertEquals(calibrator.getBiasX(), biasX, 0.0);
18147         assertEquals(calibrator.getBiasY(), biasY, 0.0);
18148         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
18149         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
18150         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18151         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18152         final Acceleration bx2 = new Acceleration(0.0,
18153                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18154         calibrator.getBiasXAsAcceleration(bx2);
18155         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18156         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18157         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
18158         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18159         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18160         final Acceleration by2 = new Acceleration(0.0,
18161                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18162         calibrator.getBiasYAsAcceleration(by2);
18163         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18164         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18165         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
18166         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18167         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18168         final Acceleration bz2 = new Acceleration(0.0,
18169                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18170         calibrator.getBiasZAsAcceleration(bz2);
18171         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18172         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18173         assertEquals(calibrator.getInitialSx(), sx, 0.0);
18174         assertEquals(calibrator.getInitialSy(), sy, 0.0);
18175         assertEquals(calibrator.getInitialSz(), sz, 0.0);
18176         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18177         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18178         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18179         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18180         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18181         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18182         final double[] bias1 = calibrator.getBias();
18183         assertArrayEquals(bias1, bias, 0.0);
18184         final double[] bias2 = new double[3];
18185         calibrator.getBias(bias2);
18186         assertArrayEquals(bias1, bias2, 0.0);
18187         final Matrix b1 = calibrator.getBiasAsMatrix();
18188         assertEquals(b1, ba);
18189         final Matrix b2 = new Matrix(3, 1);
18190         calibrator.getBiasAsMatrix(b2);
18191         assertEquals(b1, b2);
18192         final Matrix ma1 = new Matrix(3, 3);
18193         ma1.setSubmatrix(0, 0,
18194                 2, 2,
18195                 new double[]{sx, myx, mzx,
18196                         mxy, sy, mzy,
18197                         mxz, myz, sz});
18198         assertEquals(calibrator.getInitialMa(), ma1);
18199         final Matrix ma2 = new Matrix(3, 3);
18200         calibrator.getInitialMa(ma2);
18201         assertEquals(ma1, ma2);
18202         assertSame(calibrator.getMeasurements(), measurements);
18203         assertTrue(calibrator.isCommonAxisUsed());
18204         assertSame(calibrator.getListener(), this);
18205         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
18206         assertFalse(calibrator.isReady());
18207         assertFalse(calibrator.isRunning());
18208         assertNull(calibrator.getEstimatedMa());
18209         assertNull(calibrator.getEstimatedSx());
18210         assertNull(calibrator.getEstimatedSy());
18211         assertNull(calibrator.getEstimatedSz());
18212         assertNull(calibrator.getEstimatedMxy());
18213         assertNull(calibrator.getEstimatedMxz());
18214         assertNull(calibrator.getEstimatedMyx());
18215         assertNull(calibrator.getEstimatedMyz());
18216         assertNull(calibrator.getEstimatedMzx());
18217         assertNull(calibrator.getEstimatedMzy());
18218         assertNull(calibrator.getEstimatedCovariance());
18219         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18220         assertNotNull(calibrator.getGroundTruthGravityNorm());
18221         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18222         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18223         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18224                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18225         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18226         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18227         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18228 
18229         // Force IllegalArgumentException
18230         calibrator = null;
18231         try {
18232             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18233                     -gravityNorm, measurements,
18234                     true, ba, ma, this);
18235             fail("IllegalArgumentException expected but not thrown");
18236         } catch (final IllegalArgumentException ignore) {
18237         }
18238         try {
18239             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18240                     gravityNorm, measurements, true,
18241                     new Matrix(1, 1), ma, this);
18242             fail("IllegalArgumentException expected but not thrown");
18243         } catch (final IllegalArgumentException ignore) {
18244         }
18245         try {
18246             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18247                     gravityNorm, measurements, true,
18248                     new Matrix(1, 3), ma, this);
18249             fail("IllegalArgumentException expected but not thrown");
18250         } catch (final IllegalArgumentException ignore) {
18251         }
18252         try {
18253             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18254                     gravityNorm, measurements, true,
18255                     ba, new Matrix(1, 3), this);
18256             fail("IllegalArgumentException expected but not thrown");
18257         } catch (final IllegalArgumentException ignore) {
18258         }
18259         try {
18260             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18261                     gravityNorm, measurements, true,
18262                     ba, new Matrix(3, 1), this);
18263             fail("IllegalArgumentException expected but not thrown");
18264         } catch (final IllegalArgumentException ignore) {
18265         }
18266         assertNull(calibrator);
18267     }
18268 
18269     @Test
18270     public void testConstructor157() throws WrongSizeException {
18271         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18272         final double latitude = Math.toRadians(
18273                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18274         final double longitude = Math.toRadians(
18275                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18276         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18277         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18278         final NEDVelocity nedVelocity = new NEDVelocity();
18279         final ECEFPosition ecefPosition = new ECEFPosition();
18280         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18281         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18282                 ecefPosition, ecefVelocity);
18283         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18284                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18285         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
18286 
18287         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
18288                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm);
18289 
18290         // check default values
18291         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
18292         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
18293         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
18294         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
18295         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
18296         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18297         final Acceleration bx2 = new Acceleration(0.0,
18298                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18299         calibrator.getBiasXAsAcceleration(bx2);
18300         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
18301         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18302         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
18303         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
18304         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18305         final Acceleration by2 = new Acceleration(0.0,
18306                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18307         calibrator.getBiasYAsAcceleration(by2);
18308         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
18309         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18310         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
18311         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
18312         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18313         final Acceleration bz2 = new Acceleration(0.0,
18314                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18315         calibrator.getBiasZAsAcceleration(bz2);
18316         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
18317         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18318         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18319         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18320         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18321         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18322         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18323         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18324         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18325         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18326         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18327         final double[] bias1 = calibrator.getBias();
18328         assertArrayEquals(bias1, new double[3], 0.0);
18329         final double[] bias2 = new double[3];
18330         calibrator.getBias(bias2);
18331         assertArrayEquals(bias1, bias2, 0.0);
18332         final Matrix b1 = calibrator.getBiasAsMatrix();
18333         assertEquals(b1, new Matrix(3, 1));
18334         final Matrix b2 = new Matrix(3, 1);
18335         calibrator.getBiasAsMatrix(b2);
18336         assertEquals(b1, b2);
18337         final Matrix ma1 = calibrator.getInitialMa();
18338         assertEquals(ma1, new Matrix(3, 3));
18339         final Matrix ma2 = new Matrix(3, 3);
18340         calibrator.getInitialMa(ma2);
18341         assertEquals(ma1, ma2);
18342         assertNull(calibrator.getMeasurements());
18343         assertFalse(calibrator.isCommonAxisUsed());
18344         assertNull(calibrator.getListener());
18345         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18346         assertFalse(calibrator.isReady());
18347         assertFalse(calibrator.isRunning());
18348         assertNull(calibrator.getEstimatedMa());
18349         assertNull(calibrator.getEstimatedSx());
18350         assertNull(calibrator.getEstimatedSy());
18351         assertNull(calibrator.getEstimatedSz());
18352         assertNull(calibrator.getEstimatedMxy());
18353         assertNull(calibrator.getEstimatedMxz());
18354         assertNull(calibrator.getEstimatedMyx());
18355         assertNull(calibrator.getEstimatedMyz());
18356         assertNull(calibrator.getEstimatedMzx());
18357         assertNull(calibrator.getEstimatedMzy());
18358         assertNull(calibrator.getEstimatedCovariance());
18359         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18360         assertNotNull(calibrator.getGroundTruthGravityNorm());
18361         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18362         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18363         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18364                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18365         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18366         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18367         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18368 
18369         // Force IllegalArgumentException
18370         final Acceleration invalidGravityNorm = new Acceleration(
18371                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18372 
18373         calibrator = null;
18374         try {
18375             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18376                     invalidGravityNorm);
18377             fail("IllegalArgumentException expected but not thrown");
18378         } catch (final IllegalArgumentException ignore) {
18379         }
18380         assertNull(calibrator);
18381     }
18382 
18383     @Test
18384     public void testConstructor158() throws WrongSizeException {
18385         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18386         final double latitude = Math.toRadians(
18387                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18388         final double longitude = Math.toRadians(
18389                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18390         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18391         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18392         final NEDVelocity nedVelocity = new NEDVelocity();
18393         final ECEFPosition ecefPosition = new ECEFPosition();
18394         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18395         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18396                 ecefPosition, ecefVelocity);
18397         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18398                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18399         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
18400 
18401         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
18402                 new KnownBiasAndGravityNormAccelerometerCalibrator(
18403                         gravityNorm, this);
18404 
18405         // check default values
18406         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
18407         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
18408         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
18409         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
18410         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
18411         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18412         final Acceleration bx2 = new Acceleration(0.0,
18413                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18414         calibrator.getBiasXAsAcceleration(bx2);
18415         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
18416         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18417         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
18418         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
18419         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18420         final Acceleration by2 = new Acceleration(0.0,
18421                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18422         calibrator.getBiasYAsAcceleration(by2);
18423         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
18424         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18425         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
18426         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
18427         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18428         final Acceleration bz2 = new Acceleration(0.0,
18429                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18430         calibrator.getBiasZAsAcceleration(bz2);
18431         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
18432         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18433         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18434         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18435         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18436         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18437         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18438         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18439         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18440         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18441         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18442         final double[] bias1 = calibrator.getBias();
18443         assertArrayEquals(bias1, new double[3], 0.0);
18444         final double[] bias2 = new double[3];
18445         calibrator.getBias(bias2);
18446         assertArrayEquals(bias1, bias2, 0.0);
18447         final Matrix b1 = calibrator.getBiasAsMatrix();
18448         assertEquals(b1, new Matrix(3, 1));
18449         final Matrix b2 = new Matrix(3, 1);
18450         calibrator.getBiasAsMatrix(b2);
18451         assertEquals(b1, b2);
18452         final Matrix ma1 = calibrator.getInitialMa();
18453         assertEquals(ma1, new Matrix(3, 3));
18454         final Matrix ma2 = new Matrix(3, 3);
18455         calibrator.getInitialMa(ma2);
18456         assertEquals(ma1, ma2);
18457         assertNull(calibrator.getMeasurements());
18458         assertFalse(calibrator.isCommonAxisUsed());
18459         assertSame(calibrator.getListener(), this);
18460         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18461         assertFalse(calibrator.isReady());
18462         assertFalse(calibrator.isRunning());
18463         assertNull(calibrator.getEstimatedMa());
18464         assertNull(calibrator.getEstimatedSx());
18465         assertNull(calibrator.getEstimatedSy());
18466         assertNull(calibrator.getEstimatedSz());
18467         assertNull(calibrator.getEstimatedMxy());
18468         assertNull(calibrator.getEstimatedMxz());
18469         assertNull(calibrator.getEstimatedMyx());
18470         assertNull(calibrator.getEstimatedMyz());
18471         assertNull(calibrator.getEstimatedMzx());
18472         assertNull(calibrator.getEstimatedMzy());
18473         assertNull(calibrator.getEstimatedCovariance());
18474         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18475         assertNotNull(calibrator.getGroundTruthGravityNorm());
18476         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18477         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18478         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18479                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18480         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18481         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18482         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18483 
18484         // Force IllegalArgumentException
18485         final Acceleration invalidGravityNorm = new Acceleration(
18486                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18487 
18488         calibrator = null;
18489         try {
18490             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18491                     invalidGravityNorm, this);
18492             fail("IllegalArgumentException expected but not thrown");
18493         } catch (final IllegalArgumentException ignore) {
18494         }
18495         assertNull(calibrator);
18496     }
18497 
18498     @Test
18499     public void testConstructor159() throws WrongSizeException {
18500         final Collection<StandardDeviationBodyKinematics> measurements =
18501                 Collections.emptyList();
18502 
18503         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18504         final double latitude = Math.toRadians(
18505                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18506         final double longitude = Math.toRadians(
18507                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18508         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18509         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18510         final NEDVelocity nedVelocity = new NEDVelocity();
18511         final ECEFPosition ecefPosition = new ECEFPosition();
18512         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18513         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18514                 ecefPosition, ecefVelocity);
18515         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18516                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18517         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
18518 
18519         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
18520                 new KnownBiasAndGravityNormAccelerometerCalibrator(
18521                         gravityNorm, measurements);
18522 
18523         // check default values
18524         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
18525         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
18526         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
18527         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
18528         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
18529         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18530         final Acceleration bx2 = new Acceleration(0.0,
18531                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18532         calibrator.getBiasXAsAcceleration(bx2);
18533         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
18534         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18535         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
18536         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
18537         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18538         final Acceleration by2 = new Acceleration(0.0,
18539                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18540         calibrator.getBiasYAsAcceleration(by2);
18541         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
18542         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18543         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
18544         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
18545         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18546         final Acceleration bz2 = new Acceleration(0.0,
18547                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18548         calibrator.getBiasZAsAcceleration(bz2);
18549         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
18550         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18551         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18552         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18553         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18554         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18555         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18556         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18557         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18558         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18559         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18560         final double[] bias1 = calibrator.getBias();
18561         assertArrayEquals(bias1, new double[3], 0.0);
18562         final double[] bias2 = new double[3];
18563         calibrator.getBias(bias2);
18564         assertArrayEquals(bias1, bias2, 0.0);
18565         final Matrix b1 = calibrator.getBiasAsMatrix();
18566         assertEquals(b1, new Matrix(3, 1));
18567         final Matrix b2 = new Matrix(3, 1);
18568         calibrator.getBiasAsMatrix(b2);
18569         assertEquals(b1, b2);
18570         final Matrix ma1 = calibrator.getInitialMa();
18571         assertEquals(ma1, new Matrix(3, 3));
18572         final Matrix ma2 = new Matrix(3, 3);
18573         calibrator.getInitialMa(ma2);
18574         assertEquals(ma1, ma2);
18575         assertSame(calibrator.getMeasurements(), measurements);
18576         assertFalse(calibrator.isCommonAxisUsed());
18577         assertNull(calibrator.getListener());
18578         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18579         assertFalse(calibrator.isReady());
18580         assertFalse(calibrator.isRunning());
18581         assertNull(calibrator.getEstimatedMa());
18582         assertNull(calibrator.getEstimatedSx());
18583         assertNull(calibrator.getEstimatedSy());
18584         assertNull(calibrator.getEstimatedSz());
18585         assertNull(calibrator.getEstimatedMxy());
18586         assertNull(calibrator.getEstimatedMxz());
18587         assertNull(calibrator.getEstimatedMyx());
18588         assertNull(calibrator.getEstimatedMyz());
18589         assertNull(calibrator.getEstimatedMzx());
18590         assertNull(calibrator.getEstimatedMzy());
18591         assertNull(calibrator.getEstimatedCovariance());
18592         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18593         assertNotNull(calibrator.getGroundTruthGravityNorm());
18594         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18595         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18596         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18597                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18598         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18599         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18600         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18601 
18602         // Force IllegalArgumentException
18603         final Acceleration invalidGravityNorm = new Acceleration(
18604                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18605 
18606         calibrator = null;
18607         try {
18608             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18609                     invalidGravityNorm, measurements);
18610             fail("IllegalArgumentException expected but not thrown");
18611         } catch (final IllegalArgumentException ignore) {
18612         }
18613         assertNull(calibrator);
18614     }
18615 
18616     @Test
18617     public void testConstructor160() throws WrongSizeException {
18618         final Collection<StandardDeviationBodyKinematics> measurements =
18619                 Collections.emptyList();
18620 
18621         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18622         final double latitude = Math.toRadians(
18623                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18624         final double longitude = Math.toRadians(
18625                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18626         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18627         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18628         final NEDVelocity nedVelocity = new NEDVelocity();
18629         final ECEFPosition ecefPosition = new ECEFPosition();
18630         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18631         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18632                 ecefPosition, ecefVelocity);
18633         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18634                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18635         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
18636 
18637         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
18638                 new KnownBiasAndGravityNormAccelerometerCalibrator(
18639                         gravityNorm, measurements, this);
18640 
18641         // check default values
18642         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
18643         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
18644         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
18645         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
18646         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
18647         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18648         final Acceleration bx2 = new Acceleration(0.0,
18649                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18650         calibrator.getBiasXAsAcceleration(bx2);
18651         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
18652         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18653         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
18654         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
18655         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18656         final Acceleration by2 = new Acceleration(0.0,
18657                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18658         calibrator.getBiasYAsAcceleration(by2);
18659         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
18660         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18661         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
18662         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
18663         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18664         final Acceleration bz2 = new Acceleration(0.0,
18665                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18666         calibrator.getBiasZAsAcceleration(bz2);
18667         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
18668         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18669         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18670         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18671         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18672         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18673         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18674         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18675         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18676         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18677         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18678         final double[] bias1 = calibrator.getBias();
18679         assertArrayEquals(bias1, new double[3], 0.0);
18680         final double[] bias2 = new double[3];
18681         calibrator.getBias(bias2);
18682         assertArrayEquals(bias1, bias2, 0.0);
18683         final Matrix b1 = calibrator.getBiasAsMatrix();
18684         assertEquals(b1, new Matrix(3, 1));
18685         final Matrix b2 = new Matrix(3, 1);
18686         calibrator.getBiasAsMatrix(b2);
18687         assertEquals(b1, b2);
18688         final Matrix ma1 = calibrator.getInitialMa();
18689         assertEquals(ma1, new Matrix(3, 3));
18690         final Matrix ma2 = new Matrix(3, 3);
18691         calibrator.getInitialMa(ma2);
18692         assertEquals(ma1, ma2);
18693         assertSame(calibrator.getMeasurements(), measurements);
18694         assertFalse(calibrator.isCommonAxisUsed());
18695         assertSame(calibrator.getListener(), this);
18696         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18697         assertFalse(calibrator.isReady());
18698         assertFalse(calibrator.isRunning());
18699         assertNull(calibrator.getEstimatedMa());
18700         assertNull(calibrator.getEstimatedSx());
18701         assertNull(calibrator.getEstimatedSy());
18702         assertNull(calibrator.getEstimatedSz());
18703         assertNull(calibrator.getEstimatedMxy());
18704         assertNull(calibrator.getEstimatedMxz());
18705         assertNull(calibrator.getEstimatedMyx());
18706         assertNull(calibrator.getEstimatedMyz());
18707         assertNull(calibrator.getEstimatedMzx());
18708         assertNull(calibrator.getEstimatedMzy());
18709         assertNull(calibrator.getEstimatedCovariance());
18710         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18711         assertNotNull(calibrator.getGroundTruthGravityNorm());
18712         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18713         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18714         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18715                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18716         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18717         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18718         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18719 
18720         // Force IllegalArgumentException
18721         final Acceleration invalidGravityNorm = new Acceleration(
18722                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18723 
18724         calibrator = null;
18725         try {
18726             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18727                     invalidGravityNorm, measurements, this);
18728             fail("IllegalArgumentException expected but not thrown");
18729         } catch (final IllegalArgumentException ignore) {
18730         }
18731         assertNull(calibrator);
18732     }
18733 
18734     @Test
18735     public void testConstructor161() throws WrongSizeException {
18736         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18737         final double latitude = Math.toRadians(
18738                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18739         final double longitude = Math.toRadians(
18740                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18741         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18742         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18743         final NEDVelocity nedVelocity = new NEDVelocity();
18744         final ECEFPosition ecefPosition = new ECEFPosition();
18745         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18746         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18747                 ecefPosition, ecefVelocity);
18748         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18749                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18750         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
18751 
18752         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
18753                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
18754                         true);
18755 
18756         // check default values
18757         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
18758         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
18759         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
18760         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
18761         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
18762         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18763         final Acceleration bx2 = new Acceleration(0.0,
18764                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18765         calibrator.getBiasXAsAcceleration(bx2);
18766         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
18767         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18768         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
18769         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
18770         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18771         final Acceleration by2 = new Acceleration(0.0,
18772                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18773         calibrator.getBiasYAsAcceleration(by2);
18774         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
18775         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18776         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
18777         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
18778         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18779         final Acceleration bz2 = new Acceleration(0.0,
18780                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18781         calibrator.getBiasZAsAcceleration(bz2);
18782         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
18783         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18784         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18785         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18786         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18787         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18788         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18789         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18790         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18791         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18792         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18793         final double[] bias1 = calibrator.getBias();
18794         assertArrayEquals(bias1, new double[3], 0.0);
18795         final double[] bias2 = new double[3];
18796         calibrator.getBias(bias2);
18797         assertArrayEquals(bias1, bias2, 0.0);
18798         final Matrix b1 = calibrator.getBiasAsMatrix();
18799         assertEquals(b1, new Matrix(3, 1));
18800         final Matrix b2 = new Matrix(3, 1);
18801         calibrator.getBiasAsMatrix(b2);
18802         assertEquals(b1, b2);
18803         final Matrix ma1 = calibrator.getInitialMa();
18804         assertEquals(ma1, new Matrix(3, 3));
18805         final Matrix ma2 = new Matrix(3, 3);
18806         calibrator.getInitialMa(ma2);
18807         assertEquals(ma1, ma2);
18808         assertNull(calibrator.getMeasurements());
18809         assertTrue(calibrator.isCommonAxisUsed());
18810         assertNull(calibrator.getListener());
18811         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
18812         assertFalse(calibrator.isReady());
18813         assertFalse(calibrator.isRunning());
18814         assertNull(calibrator.getEstimatedMa());
18815         assertNull(calibrator.getEstimatedSx());
18816         assertNull(calibrator.getEstimatedSy());
18817         assertNull(calibrator.getEstimatedSz());
18818         assertNull(calibrator.getEstimatedMxy());
18819         assertNull(calibrator.getEstimatedMxz());
18820         assertNull(calibrator.getEstimatedMyx());
18821         assertNull(calibrator.getEstimatedMyz());
18822         assertNull(calibrator.getEstimatedMzx());
18823         assertNull(calibrator.getEstimatedMzy());
18824         assertNull(calibrator.getEstimatedCovariance());
18825         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18826         assertNotNull(calibrator.getGroundTruthGravityNorm());
18827         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18828         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18829         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18830                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18831         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18832         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18833         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18834 
18835         // Force IllegalArgumentException
18836         final Acceleration invalidGravityNorm = new Acceleration(
18837                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18838 
18839         calibrator = null;
18840         try {
18841             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18842                     invalidGravityNorm, true);
18843             fail("IllegalArgumentException expected but not thrown");
18844         } catch (final IllegalArgumentException ignore) {
18845         }
18846         assertNull(calibrator);
18847     }
18848 
18849     @Test
18850     public void testConstructor162() throws WrongSizeException {
18851         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18852         final double latitude = Math.toRadians(
18853                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18854         final double longitude = Math.toRadians(
18855                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18856         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18857         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18858         final NEDVelocity nedVelocity = new NEDVelocity();
18859         final ECEFPosition ecefPosition = new ECEFPosition();
18860         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18861         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18862                 ecefPosition, ecefVelocity);
18863         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18864                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18865         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
18866 
18867         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
18868                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
18869                         true, this);
18870 
18871         // check default values
18872         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
18873         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
18874         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
18875         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
18876         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
18877         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18878         final Acceleration bx2 = new Acceleration(0.0,
18879                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18880         calibrator.getBiasXAsAcceleration(bx2);
18881         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
18882         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18883         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
18884         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
18885         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18886         final Acceleration by2 = new Acceleration(0.0,
18887                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18888         calibrator.getBiasYAsAcceleration(by2);
18889         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
18890         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18891         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
18892         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
18893         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18894         final Acceleration bz2 = new Acceleration(0.0,
18895                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18896         calibrator.getBiasZAsAcceleration(bz2);
18897         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
18898         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18899         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18900         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18901         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18902         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18903         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18904         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18905         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18906         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18907         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18908         final double[] bias1 = calibrator.getBias();
18909         assertArrayEquals(bias1, new double[3], 0.0);
18910         final double[] bias2 = new double[3];
18911         calibrator.getBias(bias2);
18912         assertArrayEquals(bias1, bias2, 0.0);
18913         final Matrix b1 = calibrator.getBiasAsMatrix();
18914         assertEquals(b1, new Matrix(3, 1));
18915         final Matrix b2 = new Matrix(3, 1);
18916         calibrator.getBiasAsMatrix(b2);
18917         assertEquals(b1, b2);
18918         final Matrix ma1 = calibrator.getInitialMa();
18919         assertEquals(ma1, new Matrix(3, 3));
18920         final Matrix ma2 = new Matrix(3, 3);
18921         calibrator.getInitialMa(ma2);
18922         assertEquals(ma1, ma2);
18923         assertNull(calibrator.getMeasurements());
18924         assertTrue(calibrator.isCommonAxisUsed());
18925         assertSame(calibrator.getListener(), this);
18926         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
18927         assertFalse(calibrator.isReady());
18928         assertFalse(calibrator.isRunning());
18929         assertNull(calibrator.getEstimatedMa());
18930         assertNull(calibrator.getEstimatedSx());
18931         assertNull(calibrator.getEstimatedSy());
18932         assertNull(calibrator.getEstimatedSz());
18933         assertNull(calibrator.getEstimatedMxy());
18934         assertNull(calibrator.getEstimatedMxz());
18935         assertNull(calibrator.getEstimatedMyx());
18936         assertNull(calibrator.getEstimatedMyz());
18937         assertNull(calibrator.getEstimatedMzx());
18938         assertNull(calibrator.getEstimatedMzy());
18939         assertNull(calibrator.getEstimatedCovariance());
18940         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18941         assertNotNull(calibrator.getGroundTruthGravityNorm());
18942         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18943         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18944         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18945                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18946         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18947         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18948         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18949 
18950         // Force IllegalArgumentException
18951         final Acceleration invalidGravityNorm = new Acceleration(
18952                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18953 
18954         calibrator = null;
18955         try {
18956             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
18957                     invalidGravityNorm, true, this);
18958             fail("IllegalArgumentException expected but not thrown");
18959         } catch (final IllegalArgumentException ignore) {
18960         }
18961         assertNull(calibrator);
18962     }
18963 
18964     @Test
18965     public void testConstructor163() throws WrongSizeException {
18966         final Collection<StandardDeviationBodyKinematics> measurements =
18967                 Collections.emptyList();
18968         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18969         final double latitude = Math.toRadians(
18970                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18971         final double longitude = Math.toRadians(
18972                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18973         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18974         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18975         final NEDVelocity nedVelocity = new NEDVelocity();
18976         final ECEFPosition ecefPosition = new ECEFPosition();
18977         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18978         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18979                 ecefPosition, ecefVelocity);
18980         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18981                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18982         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
18983 
18984         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
18985                 new KnownBiasAndGravityNormAccelerometerCalibrator(
18986                         gravityNorm, measurements,
18987                         true);
18988 
18989         // check default values
18990         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
18991         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
18992         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
18993         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
18994         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
18995         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18996         final Acceleration bx2 = new Acceleration(0.0,
18997                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18998         calibrator.getBiasXAsAcceleration(bx2);
18999         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19000         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19001         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
19002         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19003         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19004         final Acceleration by2 = new Acceleration(0.0,
19005                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19006         calibrator.getBiasYAsAcceleration(by2);
19007         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19008         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19009         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
19010         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19011         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19012         final Acceleration bz2 = new Acceleration(0.0,
19013                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19014         calibrator.getBiasZAsAcceleration(bz2);
19015         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19016         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19017         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19018         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19019         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19020         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19021         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19022         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19023         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19024         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19025         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19026         final double[] bias1 = calibrator.getBias();
19027         assertArrayEquals(bias1, new double[3], 0.0);
19028         final double[] bias2 = new double[3];
19029         calibrator.getBias(bias2);
19030         assertArrayEquals(bias1, bias2, 0.0);
19031         final Matrix b1 = calibrator.getBiasAsMatrix();
19032         assertEquals(b1, new Matrix(3, 1));
19033         final Matrix b2 = new Matrix(3, 1);
19034         calibrator.getBiasAsMatrix(b2);
19035         assertEquals(b1, b2);
19036         final Matrix ma1 = calibrator.getInitialMa();
19037         assertEquals(ma1, new Matrix(3, 3));
19038         final Matrix ma2 = new Matrix(3, 3);
19039         calibrator.getInitialMa(ma2);
19040         assertEquals(ma1, ma2);
19041         assertSame(calibrator.getMeasurements(), measurements);
19042         assertTrue(calibrator.isCommonAxisUsed());
19043         assertNull(calibrator.getListener());
19044         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
19045         assertFalse(calibrator.isReady());
19046         assertFalse(calibrator.isRunning());
19047         assertNull(calibrator.getEstimatedMa());
19048         assertNull(calibrator.getEstimatedSx());
19049         assertNull(calibrator.getEstimatedSy());
19050         assertNull(calibrator.getEstimatedSz());
19051         assertNull(calibrator.getEstimatedMxy());
19052         assertNull(calibrator.getEstimatedMxz());
19053         assertNull(calibrator.getEstimatedMyx());
19054         assertNull(calibrator.getEstimatedMyz());
19055         assertNull(calibrator.getEstimatedMzx());
19056         assertNull(calibrator.getEstimatedMzy());
19057         assertNull(calibrator.getEstimatedCovariance());
19058         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19059         assertNotNull(calibrator.getGroundTruthGravityNorm());
19060         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19061         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19062         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19063                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19064         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19065         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19066         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19067 
19068         // Force IllegalArgumentException
19069         final Acceleration invalidGravityNorm = new Acceleration(
19070                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19071 
19072         calibrator = null;
19073         try {
19074             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
19075                     invalidGravityNorm, measurements,
19076                     true);
19077             fail("IllegalArgumentException expected but not thrown");
19078         } catch (final IllegalArgumentException ignore) {
19079         }
19080         assertNull(calibrator);
19081     }
19082 
19083     @Test
19084     public void testConstructor164() throws WrongSizeException {
19085         final Collection<StandardDeviationBodyKinematics> measurements =
19086                 Collections.emptyList();
19087         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19088         final double latitude = Math.toRadians(
19089                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19090         final double longitude = Math.toRadians(
19091                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19092         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19093         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19094         final NEDVelocity nedVelocity = new NEDVelocity();
19095         final ECEFPosition ecefPosition = new ECEFPosition();
19096         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19097         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19098                 ecefPosition, ecefVelocity);
19099         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19100                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19101         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
19102 
19103         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
19104                 new KnownBiasAndGravityNormAccelerometerCalibrator(
19105                         gravityNorm, measurements,
19106                         true, this);
19107 
19108         // check default values
19109         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
19110         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
19111         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
19112         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
19113         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
19114         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19115         final Acceleration bx2 = new Acceleration(0.0,
19116                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19117         calibrator.getBiasXAsAcceleration(bx2);
19118         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19119         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19120         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
19121         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19122         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19123         final Acceleration by2 = new Acceleration(0.0,
19124                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19125         calibrator.getBiasYAsAcceleration(by2);
19126         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19127         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19128         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
19129         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19130         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19131         final Acceleration bz2 = new Acceleration(0.0,
19132                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19133         calibrator.getBiasZAsAcceleration(bz2);
19134         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19135         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19136         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19137         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19138         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19139         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19140         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19141         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19142         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19143         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19144         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19145         final double[] bias1 = calibrator.getBias();
19146         assertArrayEquals(bias1, new double[3], 0.0);
19147         final double[] bias2 = new double[3];
19148         calibrator.getBias(bias2);
19149         assertArrayEquals(bias1, bias2, 0.0);
19150         final Matrix b1 = calibrator.getBiasAsMatrix();
19151         assertEquals(b1, new Matrix(3, 1));
19152         final Matrix b2 = new Matrix(3, 1);
19153         calibrator.getBiasAsMatrix(b2);
19154         assertEquals(b1, b2);
19155         final Matrix ma1 = calibrator.getInitialMa();
19156         assertEquals(ma1, new Matrix(3, 3));
19157         final Matrix ma2 = new Matrix(3, 3);
19158         calibrator.getInitialMa(ma2);
19159         assertEquals(ma1, ma2);
19160         assertSame(calibrator.getMeasurements(), measurements);
19161         assertTrue(calibrator.isCommonAxisUsed());
19162         assertSame(calibrator.getListener(), this);
19163         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
19164         assertFalse(calibrator.isReady());
19165         assertFalse(calibrator.isRunning());
19166         assertNull(calibrator.getEstimatedMa());
19167         assertNull(calibrator.getEstimatedSx());
19168         assertNull(calibrator.getEstimatedSy());
19169         assertNull(calibrator.getEstimatedSz());
19170         assertNull(calibrator.getEstimatedMxy());
19171         assertNull(calibrator.getEstimatedMxz());
19172         assertNull(calibrator.getEstimatedMyx());
19173         assertNull(calibrator.getEstimatedMyz());
19174         assertNull(calibrator.getEstimatedMzx());
19175         assertNull(calibrator.getEstimatedMzy());
19176         assertNull(calibrator.getEstimatedCovariance());
19177         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19178         assertNotNull(calibrator.getGroundTruthGravityNorm());
19179         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19180         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19181         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19182                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19183         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19184         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19185         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19186 
19187         // Force IllegalArgumentException
19188         final Acceleration invalidGravityNorm = new Acceleration(
19189                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19190 
19191         calibrator = null;
19192         try {
19193             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
19194                     invalidGravityNorm, measurements,
19195                     true, this);
19196             fail("IllegalArgumentException expected but not thrown");
19197         } catch (final IllegalArgumentException ignore) {
19198         }
19199         assertNull(calibrator);
19200     }
19201 
19202     @Test
19203     public void testConstructor165() throws WrongSizeException {
19204         final Matrix ba = generateBa();
19205         final double biasX = ba.getElementAtIndex(0);
19206         final double biasY = ba.getElementAtIndex(1);
19207         final double biasZ = ba.getElementAtIndex(2);
19208 
19209         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19210         final double latitude = Math.toRadians(
19211                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19212         final double longitude = Math.toRadians(
19213                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19214         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19215         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19216         final NEDVelocity nedVelocity = new NEDVelocity();
19217         final ECEFPosition ecefPosition = new ECEFPosition();
19218         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19219         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19220                 ecefPosition, ecefVelocity);
19221         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19222                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19223         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
19224 
19225         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
19226                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
19227                         biasX, biasY, biasZ);
19228 
19229         // check default values
19230         assertEquals(calibrator.getBiasX(), biasX, 0.0);
19231         assertEquals(calibrator.getBiasY(), biasY, 0.0);
19232         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
19233         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
19234         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19235         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19236         final Acceleration bx2 = new Acceleration(0.0,
19237                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19238         calibrator.getBiasXAsAcceleration(bx2);
19239         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19240         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19241         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
19242         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19243         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19244         final Acceleration by2 = new Acceleration(0.0,
19245                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19246         calibrator.getBiasYAsAcceleration(by2);
19247         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19248         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19249         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
19250         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19251         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19252         final Acceleration bz2 = new Acceleration(0.0,
19253                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19254         calibrator.getBiasZAsAcceleration(bz2);
19255         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19256         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19257         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19258         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19259         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19260         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19261         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19262         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19263         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19264         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19265         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19266         final double[] bias1 = calibrator.getBias();
19267         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19268         final double[] bias2 = new double[3];
19269         calibrator.getBias(bias2);
19270         assertArrayEquals(bias1, bias2, 0.0);
19271         final Matrix b1 = calibrator.getBiasAsMatrix();
19272         assertEquals(b1, ba);
19273         final Matrix b2 = new Matrix(3, 1);
19274         calibrator.getBiasAsMatrix(b2);
19275         assertEquals(b1, b2);
19276         final Matrix ma1 = calibrator.getInitialMa();
19277         assertEquals(ma1, new Matrix(3, 3));
19278         final Matrix ma2 = new Matrix(3, 3);
19279         calibrator.getInitialMa(ma2);
19280         assertEquals(ma1, ma2);
19281         assertNull(calibrator.getMeasurements());
19282         assertFalse(calibrator.isCommonAxisUsed());
19283         assertNull(calibrator.getListener());
19284         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19285         assertFalse(calibrator.isReady());
19286         assertFalse(calibrator.isRunning());
19287         assertNull(calibrator.getEstimatedMa());
19288         assertNull(calibrator.getEstimatedSx());
19289         assertNull(calibrator.getEstimatedSy());
19290         assertNull(calibrator.getEstimatedSz());
19291         assertNull(calibrator.getEstimatedMxy());
19292         assertNull(calibrator.getEstimatedMxz());
19293         assertNull(calibrator.getEstimatedMyx());
19294         assertNull(calibrator.getEstimatedMyz());
19295         assertNull(calibrator.getEstimatedMzx());
19296         assertNull(calibrator.getEstimatedMzy());
19297         assertNull(calibrator.getEstimatedCovariance());
19298         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19299         assertNotNull(calibrator.getGroundTruthGravityNorm());
19300         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19301         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19302         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19303                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19304         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19305         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19306         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19307 
19308         // Force IllegalArgumentException
19309         final Acceleration invalidGravityNorm = new Acceleration(
19310                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19311 
19312         calibrator = null;
19313         try {
19314             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
19315                     invalidGravityNorm, biasX, biasY, biasZ);
19316             fail("IllegalArgumentException expected but not thrown");
19317         } catch (final IllegalArgumentException ignore) {
19318         }
19319         assertNull(calibrator);
19320     }
19321 
19322     @Test
19323     public void testConstructor166() throws WrongSizeException {
19324         final Matrix ba = generateBa();
19325         final double biasX = ba.getElementAtIndex(0);
19326         final double biasY = ba.getElementAtIndex(1);
19327         final double biasZ = ba.getElementAtIndex(2);
19328 
19329         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19330         final double latitude = Math.toRadians(
19331                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19332         final double longitude = Math.toRadians(
19333                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19334         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19335         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19336         final NEDVelocity nedVelocity = new NEDVelocity();
19337         final ECEFPosition ecefPosition = new ECEFPosition();
19338         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19339         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19340                 ecefPosition, ecefVelocity);
19341         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19342                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19343         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
19344 
19345         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
19346                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
19347                         biasX, biasY, biasZ, this);
19348 
19349         // check default values
19350         assertEquals(calibrator.getBiasX(), biasX, 0.0);
19351         assertEquals(calibrator.getBiasY(), biasY, 0.0);
19352         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
19353         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
19354         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19355         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19356         final Acceleration bx2 = new Acceleration(0.0,
19357                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19358         calibrator.getBiasXAsAcceleration(bx2);
19359         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19360         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19361         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
19362         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19363         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19364         final Acceleration by2 = new Acceleration(0.0,
19365                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19366         calibrator.getBiasYAsAcceleration(by2);
19367         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19368         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19369         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
19370         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19371         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19372         final Acceleration bz2 = new Acceleration(0.0,
19373                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19374         calibrator.getBiasZAsAcceleration(bz2);
19375         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19376         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19377         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19378         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19379         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19380         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19381         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19382         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19383         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19384         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19385         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19386         final double[] bias1 = calibrator.getBias();
19387         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19388         final double[] bias2 = new double[3];
19389         calibrator.getBias(bias2);
19390         assertArrayEquals(bias1, bias2, 0.0);
19391         final Matrix b1 = calibrator.getBiasAsMatrix();
19392         assertEquals(b1, ba);
19393         final Matrix b2 = new Matrix(3, 1);
19394         calibrator.getBiasAsMatrix(b2);
19395         assertEquals(b1, b2);
19396         final Matrix ma1 = calibrator.getInitialMa();
19397         assertEquals(ma1, new Matrix(3, 3));
19398         final Matrix ma2 = new Matrix(3, 3);
19399         calibrator.getInitialMa(ma2);
19400         assertEquals(ma1, ma2);
19401         assertNull(calibrator.getMeasurements());
19402         assertFalse(calibrator.isCommonAxisUsed());
19403         assertSame(calibrator.getListener(), this);
19404         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19405         assertFalse(calibrator.isReady());
19406         assertFalse(calibrator.isRunning());
19407         assertNull(calibrator.getEstimatedMa());
19408         assertNull(calibrator.getEstimatedSx());
19409         assertNull(calibrator.getEstimatedSy());
19410         assertNull(calibrator.getEstimatedSz());
19411         assertNull(calibrator.getEstimatedMxy());
19412         assertNull(calibrator.getEstimatedMxz());
19413         assertNull(calibrator.getEstimatedMyx());
19414         assertNull(calibrator.getEstimatedMyz());
19415         assertNull(calibrator.getEstimatedMzx());
19416         assertNull(calibrator.getEstimatedMzy());
19417         assertNull(calibrator.getEstimatedCovariance());
19418         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19419         assertNotNull(calibrator.getGroundTruthGravityNorm());
19420         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19421         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19422         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19423                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19424         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19425         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19426         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19427 
19428         // Force IllegalArgumentException
19429         final Acceleration invalidGravityNorm = new Acceleration(
19430                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19431 
19432         calibrator = null;
19433         try {
19434             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
19435                     invalidGravityNorm, biasX, biasY, biasZ,
19436                     this);
19437             fail("IllegalArgumentException expected but not thrown");
19438         } catch (final IllegalArgumentException ignore) {
19439         }
19440         assertNull(calibrator);
19441     }
19442 
19443     @Test
19444     public void testConstructor167() throws WrongSizeException {
19445         final Collection<StandardDeviationBodyKinematics> measurements =
19446                 Collections.emptyList();
19447 
19448         final Matrix ba = generateBa();
19449         final double biasX = ba.getElementAtIndex(0);
19450         final double biasY = ba.getElementAtIndex(1);
19451         final double biasZ = ba.getElementAtIndex(2);
19452 
19453         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19454         final double latitude = Math.toRadians(
19455                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19456         final double longitude = Math.toRadians(
19457                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19458         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19459         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19460         final NEDVelocity nedVelocity = new NEDVelocity();
19461         final ECEFPosition ecefPosition = new ECEFPosition();
19462         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19463         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19464                 ecefPosition, ecefVelocity);
19465         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19466                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19467         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
19468 
19469         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
19470                 new KnownBiasAndGravityNormAccelerometerCalibrator(
19471                         gravityNorm, measurements,
19472                         biasX, biasY, biasZ);
19473 
19474         // check default values
19475         assertEquals(calibrator.getBiasX(), biasX, 0.0);
19476         assertEquals(calibrator.getBiasY(), biasY, 0.0);
19477         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
19478         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
19479         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19480         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19481         final Acceleration bx2 = new Acceleration(0.0,
19482                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19483         calibrator.getBiasXAsAcceleration(bx2);
19484         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19485         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19486         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
19487         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19488         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19489         final Acceleration by2 = new Acceleration(0.0,
19490                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19491         calibrator.getBiasYAsAcceleration(by2);
19492         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19493         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19494         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
19495         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19496         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19497         final Acceleration bz2 = new Acceleration(0.0,
19498                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19499         calibrator.getBiasZAsAcceleration(bz2);
19500         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19501         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19502         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19503         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19504         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19505         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19506         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19507         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19508         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19509         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19510         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19511         final double[] bias1 = calibrator.getBias();
19512         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19513         final double[] bias2 = new double[3];
19514         calibrator.getBias(bias2);
19515         assertArrayEquals(bias1, bias2, 0.0);
19516         final Matrix b1 = calibrator.getBiasAsMatrix();
19517         assertEquals(b1, ba);
19518         final Matrix b2 = new Matrix(3, 1);
19519         calibrator.getBiasAsMatrix(b2);
19520         assertEquals(b1, b2);
19521         final Matrix ma1 = calibrator.getInitialMa();
19522         assertEquals(ma1, new Matrix(3, 3));
19523         final Matrix ma2 = new Matrix(3, 3);
19524         calibrator.getInitialMa(ma2);
19525         assertEquals(ma1, ma2);
19526         assertSame(calibrator.getMeasurements(), measurements);
19527         assertFalse(calibrator.isCommonAxisUsed());
19528         assertNull(calibrator.getListener());
19529         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19530         assertFalse(calibrator.isReady());
19531         assertFalse(calibrator.isRunning());
19532         assertNull(calibrator.getEstimatedMa());
19533         assertNull(calibrator.getEstimatedSx());
19534         assertNull(calibrator.getEstimatedSy());
19535         assertNull(calibrator.getEstimatedSz());
19536         assertNull(calibrator.getEstimatedMxy());
19537         assertNull(calibrator.getEstimatedMxz());
19538         assertNull(calibrator.getEstimatedMyx());
19539         assertNull(calibrator.getEstimatedMyz());
19540         assertNull(calibrator.getEstimatedMzx());
19541         assertNull(calibrator.getEstimatedMzy());
19542         assertNull(calibrator.getEstimatedCovariance());
19543         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19544         assertNotNull(calibrator.getGroundTruthGravityNorm());
19545         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19546         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19547         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19548                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19549         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19550         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19551         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19552 
19553         // Force IllegalArgumentException
19554         final Acceleration invalidGravityNorm = new Acceleration(
19555                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19556 
19557         calibrator = null;
19558         try {
19559             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
19560                     invalidGravityNorm, measurements,
19561                     biasX, biasY, biasZ);
19562             fail("IllegalArgumentException expected but not thrown");
19563         } catch (final IllegalArgumentException ignore) {
19564         }
19565         assertNull(calibrator);
19566     }
19567 
19568     @Test
19569     public void testConstructor168() throws WrongSizeException {
19570         final Collection<StandardDeviationBodyKinematics> measurements =
19571                 Collections.emptyList();
19572 
19573         final Matrix ba = generateBa();
19574         final double biasX = ba.getElementAtIndex(0);
19575         final double biasY = ba.getElementAtIndex(1);
19576         final double biasZ = ba.getElementAtIndex(2);
19577 
19578         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19579         final double latitude = Math.toRadians(
19580                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19581         final double longitude = Math.toRadians(
19582                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19583         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19584         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19585         final NEDVelocity nedVelocity = new NEDVelocity();
19586         final ECEFPosition ecefPosition = new ECEFPosition();
19587         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19588         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19589                 ecefPosition, ecefVelocity);
19590         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19591                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19592         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
19593 
19594         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
19595                 new KnownBiasAndGravityNormAccelerometerCalibrator(
19596                         gravityNorm, measurements,
19597                         biasX, biasY, biasZ, this);
19598 
19599         // check default values
19600         assertEquals(calibrator.getBiasX(), biasX, 0.0);
19601         assertEquals(calibrator.getBiasY(), biasY, 0.0);
19602         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
19603         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
19604         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19605         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19606         final Acceleration bx2 = new Acceleration(0.0,
19607                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19608         calibrator.getBiasXAsAcceleration(bx2);
19609         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19610         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19611         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
19612         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19613         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19614         final Acceleration by2 = new Acceleration(0.0,
19615                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19616         calibrator.getBiasYAsAcceleration(by2);
19617         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19618         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19619         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
19620         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19621         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19622         final Acceleration bz2 = new Acceleration(0.0,
19623                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19624         calibrator.getBiasZAsAcceleration(bz2);
19625         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19626         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19627         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19628         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19629         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19630         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19631         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19632         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19633         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19634         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19635         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19636         final double[] bias1 = calibrator.getBias();
19637         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19638         final double[] bias2 = new double[3];
19639         calibrator.getBias(bias2);
19640         assertArrayEquals(bias1, bias2, 0.0);
19641         final Matrix b1 = calibrator.getBiasAsMatrix();
19642         assertEquals(b1, ba);
19643         final Matrix b2 = new Matrix(3, 1);
19644         calibrator.getBiasAsMatrix(b2);
19645         assertEquals(b1, b2);
19646         final Matrix ma1 = calibrator.getInitialMa();
19647         assertEquals(ma1, new Matrix(3, 3));
19648         final Matrix ma2 = new Matrix(3, 3);
19649         calibrator.getInitialMa(ma2);
19650         assertEquals(ma1, ma2);
19651         assertSame(calibrator.getMeasurements(), measurements);
19652         assertFalse(calibrator.isCommonAxisUsed());
19653         assertSame(calibrator.getListener(), this);
19654         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19655         assertFalse(calibrator.isReady());
19656         assertFalse(calibrator.isRunning());
19657         assertNull(calibrator.getEstimatedMa());
19658         assertNull(calibrator.getEstimatedSx());
19659         assertNull(calibrator.getEstimatedSy());
19660         assertNull(calibrator.getEstimatedSz());
19661         assertNull(calibrator.getEstimatedMxy());
19662         assertNull(calibrator.getEstimatedMxz());
19663         assertNull(calibrator.getEstimatedMyx());
19664         assertNull(calibrator.getEstimatedMyz());
19665         assertNull(calibrator.getEstimatedMzx());
19666         assertNull(calibrator.getEstimatedMzy());
19667         assertNull(calibrator.getEstimatedCovariance());
19668         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19669         assertNotNull(calibrator.getGroundTruthGravityNorm());
19670         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19671         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19672         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19673                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19674         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19675         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19676         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19677 
19678         // Force IllegalArgumentException
19679         final Acceleration invalidGravityNorm = new Acceleration(
19680                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19681 
19682         calibrator = null;
19683         try {
19684             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
19685                     invalidGravityNorm, measurements,
19686                     biasX, biasY, biasZ, this);
19687             fail("IllegalArgumentException expected but not thrown");
19688         } catch (final IllegalArgumentException ignore) {
19689         }
19690         assertNull(calibrator);
19691     }
19692 
19693     @Test
19694     public void testConstructor169() throws WrongSizeException {
19695         final Matrix ba = generateBa();
19696         final double biasX = ba.getElementAtIndex(0);
19697         final double biasY = ba.getElementAtIndex(1);
19698         final double biasZ = ba.getElementAtIndex(2);
19699 
19700         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19701         final double latitude = Math.toRadians(
19702                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19703         final double longitude = Math.toRadians(
19704                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19705         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19706         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19707         final NEDVelocity nedVelocity = new NEDVelocity();
19708         final ECEFPosition ecefPosition = new ECEFPosition();
19709         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19710         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19711                 ecefPosition, ecefVelocity);
19712         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19713                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19714         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
19715 
19716         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
19717                 new KnownBiasAndGravityNormAccelerometerCalibrator(
19718                         gravityNorm, true, biasX, biasY, biasZ);
19719 
19720         // check default values
19721         assertEquals(calibrator.getBiasX(), biasX, 0.0);
19722         assertEquals(calibrator.getBiasY(), biasY, 0.0);
19723         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
19724         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
19725         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19726         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19727         final Acceleration bx2 = new Acceleration(0.0,
19728                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19729         calibrator.getBiasXAsAcceleration(bx2);
19730         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19731         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19732         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
19733         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19734         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19735         final Acceleration by2 = new Acceleration(0.0,
19736                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19737         calibrator.getBiasYAsAcceleration(by2);
19738         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19739         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19740         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
19741         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19742         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19743         final Acceleration bz2 = new Acceleration(0.0,
19744                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19745         calibrator.getBiasZAsAcceleration(bz2);
19746         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19747         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19748         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19749         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19750         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19751         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19752         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19753         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19754         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19755         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19756         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19757         final double[] bias1 = calibrator.getBias();
19758         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19759         final double[] bias2 = new double[3];
19760         calibrator.getBias(bias2);
19761         assertArrayEquals(bias1, bias2, 0.0);
19762         final Matrix b1 = calibrator.getBiasAsMatrix();
19763         assertEquals(b1, ba);
19764         final Matrix b2 = new Matrix(3, 1);
19765         calibrator.getBiasAsMatrix(b2);
19766         assertEquals(b1, b2);
19767         final Matrix ma1 = calibrator.getInitialMa();
19768         assertEquals(ma1, new Matrix(3, 3));
19769         final Matrix ma2 = new Matrix(3, 3);
19770         calibrator.getInitialMa(ma2);
19771         assertEquals(ma1, ma2);
19772         assertNull(calibrator.getMeasurements());
19773         assertTrue(calibrator.isCommonAxisUsed());
19774         assertNull(calibrator.getListener());
19775         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
19776         assertFalse(calibrator.isReady());
19777         assertFalse(calibrator.isRunning());
19778         assertNull(calibrator.getEstimatedMa());
19779         assertNull(calibrator.getEstimatedSx());
19780         assertNull(calibrator.getEstimatedSy());
19781         assertNull(calibrator.getEstimatedSz());
19782         assertNull(calibrator.getEstimatedMxy());
19783         assertNull(calibrator.getEstimatedMxz());
19784         assertNull(calibrator.getEstimatedMyx());
19785         assertNull(calibrator.getEstimatedMyz());
19786         assertNull(calibrator.getEstimatedMzx());
19787         assertNull(calibrator.getEstimatedMzy());
19788         assertNull(calibrator.getEstimatedCovariance());
19789         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19790         assertNotNull(calibrator.getGroundTruthGravityNorm());
19791         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19792         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19793         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19794                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19795         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19796         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19797         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19798 
19799         // Force IllegalArgumentException
19800         final Acceleration invalidGravityNorm = new Acceleration(
19801                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19802 
19803         calibrator = null;
19804         try {
19805             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
19806                     invalidGravityNorm, true, biasX, biasY, biasZ);
19807             fail("IllegalArgumentException expected but not thrown");
19808         } catch (final IllegalArgumentException ignore) {
19809         }
19810         assertNull(calibrator);
19811     }
19812 
19813     @Test
19814     public void testConstructor170() throws WrongSizeException {
19815         final Matrix ba = generateBa();
19816         final double biasX = ba.getElementAtIndex(0);
19817         final double biasY = ba.getElementAtIndex(1);
19818         final double biasZ = ba.getElementAtIndex(2);
19819 
19820         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19821         final double latitude = Math.toRadians(
19822                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19823         final double longitude = Math.toRadians(
19824                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19825         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19826         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19827         final NEDVelocity nedVelocity = new NEDVelocity();
19828         final ECEFPosition ecefPosition = new ECEFPosition();
19829         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19830         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19831                 ecefPosition, ecefVelocity);
19832         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19833                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19834         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
19835 
19836         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
19837                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
19838                         true, biasX, biasY, biasZ, this);
19839 
19840         // check default values
19841         assertEquals(calibrator.getBiasX(), biasX, 0.0);
19842         assertEquals(calibrator.getBiasY(), biasY, 0.0);
19843         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
19844         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
19845         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19846         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19847         final Acceleration bx2 = new Acceleration(0.0,
19848                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19849         calibrator.getBiasXAsAcceleration(bx2);
19850         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19851         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19852         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
19853         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19854         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19855         final Acceleration by2 = new Acceleration(0.0,
19856                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19857         calibrator.getBiasYAsAcceleration(by2);
19858         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19859         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19860         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
19861         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19862         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19863         final Acceleration bz2 = new Acceleration(0.0,
19864                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19865         calibrator.getBiasZAsAcceleration(bz2);
19866         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19867         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19868         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19869         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19870         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19871         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19872         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19873         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19874         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19875         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19876         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19877         final double[] bias1 = calibrator.getBias();
19878         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19879         final double[] bias2 = new double[3];
19880         calibrator.getBias(bias2);
19881         assertArrayEquals(bias1, bias2, 0.0);
19882         final Matrix b1 = calibrator.getBiasAsMatrix();
19883         assertEquals(b1, ba);
19884         final Matrix b2 = new Matrix(3, 1);
19885         calibrator.getBiasAsMatrix(b2);
19886         assertEquals(b1, b2);
19887         final Matrix ma1 = calibrator.getInitialMa();
19888         assertEquals(ma1, new Matrix(3, 3));
19889         final Matrix ma2 = new Matrix(3, 3);
19890         calibrator.getInitialMa(ma2);
19891         assertEquals(ma1, ma2);
19892         assertNull(calibrator.getMeasurements());
19893         assertTrue(calibrator.isCommonAxisUsed());
19894         assertSame(calibrator.getListener(), this);
19895         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
19896         assertFalse(calibrator.isReady());
19897         assertFalse(calibrator.isRunning());
19898         assertNull(calibrator.getEstimatedMa());
19899         assertNull(calibrator.getEstimatedSx());
19900         assertNull(calibrator.getEstimatedSy());
19901         assertNull(calibrator.getEstimatedSz());
19902         assertNull(calibrator.getEstimatedMxy());
19903         assertNull(calibrator.getEstimatedMxz());
19904         assertNull(calibrator.getEstimatedMyx());
19905         assertNull(calibrator.getEstimatedMyz());
19906         assertNull(calibrator.getEstimatedMzx());
19907         assertNull(calibrator.getEstimatedMzy());
19908         assertNull(calibrator.getEstimatedCovariance());
19909         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19910         assertNotNull(calibrator.getGroundTruthGravityNorm());
19911         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19912         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19913         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19914                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19915         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19916         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19917         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19918 
19919         // Force IllegalArgumentException
19920         final Acceleration invalidGravityNorm = new Acceleration(
19921                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19922 
19923         calibrator = null;
19924         try {
19925             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
19926                     invalidGravityNorm, true,
19927                     biasX, biasY, biasZ, this);
19928             fail("IllegalArgumentException expected but not thrown");
19929         } catch (final IllegalArgumentException ignore) {
19930         }
19931         assertNull(calibrator);
19932     }
19933 
19934     @Test
19935     public void testConstructor171() throws WrongSizeException {
19936         final Collection<StandardDeviationBodyKinematics> measurements =
19937                 Collections.emptyList();
19938 
19939         final Matrix ba = generateBa();
19940         final double biasX = ba.getElementAtIndex(0);
19941         final double biasY = ba.getElementAtIndex(1);
19942         final double biasZ = ba.getElementAtIndex(2);
19943 
19944         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19945         final double latitude = Math.toRadians(
19946                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19947         final double longitude = Math.toRadians(
19948                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19949         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19950         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19951         final NEDVelocity nedVelocity = new NEDVelocity();
19952         final ECEFPosition ecefPosition = new ECEFPosition();
19953         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19954         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19955                 ecefPosition, ecefVelocity);
19956         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19957                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19958         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
19959 
19960         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
19961                 new KnownBiasAndGravityNormAccelerometerCalibrator(
19962                         gravityNorm, measurements,
19963                         true, biasX, biasY, biasZ);
19964 
19965         // check default values
19966         assertEquals(calibrator.getBiasX(), biasX, 0.0);
19967         assertEquals(calibrator.getBiasY(), biasY, 0.0);
19968         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
19969         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
19970         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19971         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19972         final Acceleration bx2 = new Acceleration(0.0,
19973                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19974         calibrator.getBiasXAsAcceleration(bx2);
19975         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19976         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19977         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
19978         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19979         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19980         final Acceleration by2 = new Acceleration(0.0,
19981                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19982         calibrator.getBiasYAsAcceleration(by2);
19983         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19984         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19985         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
19986         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19987         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19988         final Acceleration bz2 = new Acceleration(0.0,
19989                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19990         calibrator.getBiasZAsAcceleration(bz2);
19991         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19992         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19993         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19994         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19995         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19996         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19997         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19998         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19999         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20000         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20001         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20002         final double[] bias1 = calibrator.getBias();
20003         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20004         final double[] bias2 = new double[3];
20005         calibrator.getBias(bias2);
20006         assertArrayEquals(bias1, bias2, 0.0);
20007         final Matrix b1 = calibrator.getBiasAsMatrix();
20008         assertEquals(b1, ba);
20009         final Matrix b2 = new Matrix(3, 1);
20010         calibrator.getBiasAsMatrix(b2);
20011         assertEquals(b1, b2);
20012         final Matrix ma1 = calibrator.getInitialMa();
20013         assertEquals(ma1, new Matrix(3, 3));
20014         final Matrix ma2 = new Matrix(3, 3);
20015         calibrator.getInitialMa(ma2);
20016         assertEquals(ma1, ma2);
20017         assertSame(calibrator.getMeasurements(), measurements);
20018         assertTrue(calibrator.isCommonAxisUsed());
20019         assertNull(calibrator.getListener());
20020         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
20021         assertFalse(calibrator.isReady());
20022         assertFalse(calibrator.isRunning());
20023         assertNull(calibrator.getEstimatedMa());
20024         assertNull(calibrator.getEstimatedSx());
20025         assertNull(calibrator.getEstimatedSy());
20026         assertNull(calibrator.getEstimatedSz());
20027         assertNull(calibrator.getEstimatedMxy());
20028         assertNull(calibrator.getEstimatedMxz());
20029         assertNull(calibrator.getEstimatedMyx());
20030         assertNull(calibrator.getEstimatedMyz());
20031         assertNull(calibrator.getEstimatedMzx());
20032         assertNull(calibrator.getEstimatedMzy());
20033         assertNull(calibrator.getEstimatedCovariance());
20034         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20035         assertNotNull(calibrator.getGroundTruthGravityNorm());
20036         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20037         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20038         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20039                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20040         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20041         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20042         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20043 
20044         // Force IllegalArgumentException
20045         final Acceleration invalidGravityNorm = new Acceleration(
20046                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20047 
20048         calibrator = null;
20049         try {
20050             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
20051                     invalidGravityNorm, measurements,
20052                     true, biasX, biasY, biasZ);
20053             fail("IllegalArgumentException expected but not thrown");
20054         } catch (final IllegalArgumentException ignore) {
20055         }
20056         assertNull(calibrator);
20057     }
20058 
20059     @Test
20060     public void testConstructor172() throws WrongSizeException {
20061         final Collection<StandardDeviationBodyKinematics> measurements =
20062                 Collections.emptyList();
20063 
20064         final Matrix ba = generateBa();
20065         final double biasX = ba.getElementAtIndex(0);
20066         final double biasY = ba.getElementAtIndex(1);
20067         final double biasZ = ba.getElementAtIndex(2);
20068 
20069         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20070         final double latitude = Math.toRadians(
20071                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20072         final double longitude = Math.toRadians(
20073                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20074         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20075         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20076         final NEDVelocity nedVelocity = new NEDVelocity();
20077         final ECEFPosition ecefPosition = new ECEFPosition();
20078         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20079         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20080                 ecefPosition, ecefVelocity);
20081         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20082                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20083         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20084 
20085         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
20086                 new KnownBiasAndGravityNormAccelerometerCalibrator(
20087                         gravityNorm, measurements,
20088                         true, biasX, biasY, biasZ, this);
20089 
20090         // check default values
20091         assertEquals(calibrator.getBiasX(), biasX, 0.0);
20092         assertEquals(calibrator.getBiasY(), biasY, 0.0);
20093         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
20094         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
20095         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20096         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20097         final Acceleration bx2 = new Acceleration(0.0,
20098                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20099         calibrator.getBiasXAsAcceleration(bx2);
20100         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20101         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20102         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
20103         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20104         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20105         final Acceleration by2 = new Acceleration(0.0,
20106                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20107         calibrator.getBiasYAsAcceleration(by2);
20108         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20109         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20110         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
20111         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20112         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20113         final Acceleration bz2 = new Acceleration(0.0,
20114                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20115         calibrator.getBiasZAsAcceleration(bz2);
20116         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20117         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20118         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20119         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20120         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20121         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20122         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20123         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20124         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20125         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20126         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20127         final double[] bias1 = calibrator.getBias();
20128         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20129         final double[] bias2 = new double[3];
20130         calibrator.getBias(bias2);
20131         assertArrayEquals(bias1, bias2, 0.0);
20132         final Matrix b1 = calibrator.getBiasAsMatrix();
20133         assertEquals(b1, ba);
20134         final Matrix b2 = new Matrix(3, 1);
20135         calibrator.getBiasAsMatrix(b2);
20136         assertEquals(b1, b2);
20137         final Matrix ma1 = calibrator.getInitialMa();
20138         assertEquals(ma1, new Matrix(3, 3));
20139         final Matrix ma2 = new Matrix(3, 3);
20140         calibrator.getInitialMa(ma2);
20141         assertEquals(ma1, ma2);
20142         assertSame(calibrator.getMeasurements(), measurements);
20143         assertTrue(calibrator.isCommonAxisUsed());
20144         assertSame(calibrator.getListener(), this);
20145         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
20146         assertFalse(calibrator.isReady());
20147         assertFalse(calibrator.isRunning());
20148         assertNull(calibrator.getEstimatedMa());
20149         assertNull(calibrator.getEstimatedSx());
20150         assertNull(calibrator.getEstimatedSy());
20151         assertNull(calibrator.getEstimatedSz());
20152         assertNull(calibrator.getEstimatedMxy());
20153         assertNull(calibrator.getEstimatedMxz());
20154         assertNull(calibrator.getEstimatedMyx());
20155         assertNull(calibrator.getEstimatedMyz());
20156         assertNull(calibrator.getEstimatedMzx());
20157         assertNull(calibrator.getEstimatedMzy());
20158         assertNull(calibrator.getEstimatedCovariance());
20159         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20160         assertNotNull(calibrator.getGroundTruthGravityNorm());
20161         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20162         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20163         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20164                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20165         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20166         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20167         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20168 
20169         // Force IllegalArgumentException
20170         final Acceleration invalidGravityNorm = new Acceleration(
20171                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20172 
20173         calibrator = null;
20174         try {
20175             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
20176                     invalidGravityNorm, measurements,
20177                     true, biasX, biasY, biasZ, this);
20178             fail("IllegalArgumentException expected but not thrown");
20179         } catch (final IllegalArgumentException ignore) {
20180         }
20181         assertNull(calibrator);
20182     }
20183 
20184     @Test
20185     public void testConstructor173() throws WrongSizeException {
20186         final Matrix ba = generateBa();
20187         final double biasX = ba.getElementAtIndex(0);
20188         final double biasY = ba.getElementAtIndex(1);
20189         final double biasZ = ba.getElementAtIndex(2);
20190 
20191         final Acceleration bx = new Acceleration(biasX,
20192                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20193         final Acceleration by = new Acceleration(biasY,
20194                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20195         final Acceleration bz = new Acceleration(biasZ,
20196                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20197 
20198         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20199         final double latitude = Math.toRadians(
20200                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20201         final double longitude = Math.toRadians(
20202                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20203         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20204         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20205         final NEDVelocity nedVelocity = new NEDVelocity();
20206         final ECEFPosition ecefPosition = new ECEFPosition();
20207         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20208         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20209                 ecefPosition, ecefVelocity);
20210         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20211                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20212         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20213 
20214         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
20215                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
20216                         bx, by, bz);
20217 
20218         // check default values
20219         assertEquals(calibrator.getBiasX(), biasX, 0.0);
20220         assertEquals(calibrator.getBiasY(), biasY, 0.0);
20221         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
20222         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
20223         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20224         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20225         final Acceleration bx2 = new Acceleration(0.0,
20226                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20227         calibrator.getBiasXAsAcceleration(bx2);
20228         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20229         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20230         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
20231         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20232         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20233         final Acceleration by2 = new Acceleration(0.0,
20234                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20235         calibrator.getBiasYAsAcceleration(by2);
20236         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20237         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20238         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
20239         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20240         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20241         final Acceleration bz2 = new Acceleration(0.0,
20242                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20243         calibrator.getBiasZAsAcceleration(bz2);
20244         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20245         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20246         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20247         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20248         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20249         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20250         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20251         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20252         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20253         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20254         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20255         final double[] bias1 = calibrator.getBias();
20256         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20257         final double[] bias2 = new double[3];
20258         calibrator.getBias(bias2);
20259         assertArrayEquals(bias1, bias2, 0.0);
20260         final Matrix b1 = calibrator.getBiasAsMatrix();
20261         assertEquals(b1, ba);
20262         final Matrix b2 = new Matrix(3, 1);
20263         calibrator.getBiasAsMatrix(b2);
20264         assertEquals(b1, b2);
20265         final Matrix ma1 = calibrator.getInitialMa();
20266         assertEquals(ma1, new Matrix(3, 3));
20267         final Matrix ma2 = new Matrix(3, 3);
20268         calibrator.getInitialMa(ma2);
20269         assertEquals(ma1, ma2);
20270         assertNull(calibrator.getMeasurements());
20271         assertFalse(calibrator.isCommonAxisUsed());
20272         assertNull(calibrator.getListener());
20273         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20274         assertFalse(calibrator.isReady());
20275         assertFalse(calibrator.isRunning());
20276         assertNull(calibrator.getEstimatedMa());
20277         assertNull(calibrator.getEstimatedSx());
20278         assertNull(calibrator.getEstimatedSy());
20279         assertNull(calibrator.getEstimatedSz());
20280         assertNull(calibrator.getEstimatedMxy());
20281         assertNull(calibrator.getEstimatedMxz());
20282         assertNull(calibrator.getEstimatedMyx());
20283         assertNull(calibrator.getEstimatedMyz());
20284         assertNull(calibrator.getEstimatedMzx());
20285         assertNull(calibrator.getEstimatedMzy());
20286         assertNull(calibrator.getEstimatedCovariance());
20287         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20288         assertNotNull(calibrator.getGroundTruthGravityNorm());
20289         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20290         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20291         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20292                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20293         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20294         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20295         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20296 
20297         // Force IllegalArgumentException
20298         final Acceleration invalidGravityNorm = new Acceleration(
20299                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20300 
20301         calibrator = null;
20302         try {
20303             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
20304                     invalidGravityNorm, bx, by, bz);
20305             fail("IllegalArgumentException expected but not thrown");
20306         } catch (final IllegalArgumentException ignore) {
20307         }
20308         assertNull(calibrator);
20309     }
20310 
20311     @Test
20312     public void testConstructor174() throws WrongSizeException {
20313         final Matrix ba = generateBa();
20314         final double biasX = ba.getElementAtIndex(0);
20315         final double biasY = ba.getElementAtIndex(1);
20316         final double biasZ = ba.getElementAtIndex(2);
20317 
20318         final Acceleration bx = new Acceleration(biasX,
20319                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20320         final Acceleration by = new Acceleration(biasY,
20321                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20322         final Acceleration bz = new Acceleration(biasZ,
20323                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20324 
20325         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20326         final double latitude = Math.toRadians(
20327                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20328         final double longitude = Math.toRadians(
20329                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20330         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20331         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20332         final NEDVelocity nedVelocity = new NEDVelocity();
20333         final ECEFPosition ecefPosition = new ECEFPosition();
20334         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20335         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20336                 ecefPosition, ecefVelocity);
20337         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20338                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20339         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20340 
20341         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
20342                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
20343                         bx, by, bz, this);
20344 
20345         // check default values
20346         assertEquals(calibrator.getBiasX(), biasX, 0.0);
20347         assertEquals(calibrator.getBiasY(), biasY, 0.0);
20348         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
20349         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
20350         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20351         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20352         final Acceleration bx2 = new Acceleration(0.0,
20353                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20354         calibrator.getBiasXAsAcceleration(bx2);
20355         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20356         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20357         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
20358         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20359         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20360         final Acceleration by2 = new Acceleration(0.0,
20361                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20362         calibrator.getBiasYAsAcceleration(by2);
20363         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20364         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20365         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
20366         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20367         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20368         final Acceleration bz2 = new Acceleration(0.0,
20369                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20370         calibrator.getBiasZAsAcceleration(bz2);
20371         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20372         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20373         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20374         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20375         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20376         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20377         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20378         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20379         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20380         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20381         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20382         final double[] bias1 = calibrator.getBias();
20383         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20384         final double[] bias2 = new double[3];
20385         calibrator.getBias(bias2);
20386         assertArrayEquals(bias1, bias2, 0.0);
20387         final Matrix b1 = calibrator.getBiasAsMatrix();
20388         assertEquals(b1, ba);
20389         final Matrix b2 = new Matrix(3, 1);
20390         calibrator.getBiasAsMatrix(b2);
20391         assertEquals(b1, b2);
20392         final Matrix ma1 = calibrator.getInitialMa();
20393         assertEquals(ma1, new Matrix(3, 3));
20394         final Matrix ma2 = new Matrix(3, 3);
20395         calibrator.getInitialMa(ma2);
20396         assertEquals(ma1, ma2);
20397         assertNull(calibrator.getMeasurements());
20398         assertFalse(calibrator.isCommonAxisUsed());
20399         assertSame(calibrator.getListener(), this);
20400         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20401         assertFalse(calibrator.isReady());
20402         assertFalse(calibrator.isRunning());
20403         assertNull(calibrator.getEstimatedMa());
20404         assertNull(calibrator.getEstimatedSx());
20405         assertNull(calibrator.getEstimatedSy());
20406         assertNull(calibrator.getEstimatedSz());
20407         assertNull(calibrator.getEstimatedMxy());
20408         assertNull(calibrator.getEstimatedMxz());
20409         assertNull(calibrator.getEstimatedMyx());
20410         assertNull(calibrator.getEstimatedMyz());
20411         assertNull(calibrator.getEstimatedMzx());
20412         assertNull(calibrator.getEstimatedMzy());
20413         assertNull(calibrator.getEstimatedCovariance());
20414         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20415         assertNotNull(calibrator.getGroundTruthGravityNorm());
20416         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20417         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20418         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20419                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20420         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20421         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20422         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20423 
20424         // Force IllegalArgumentException
20425         final Acceleration invalidGravityNorm = new Acceleration(
20426                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20427 
20428         calibrator = null;
20429         try {
20430             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
20431                     invalidGravityNorm, bx, by, bz, this);
20432             fail("IllegalArgumentException expected but not thrown");
20433         } catch (final IllegalArgumentException ignore) {
20434         }
20435         assertNull(calibrator);
20436     }
20437 
20438     @Test
20439     public void testConstructor175() throws WrongSizeException {
20440         final Collection<StandardDeviationBodyKinematics> measurements =
20441                 Collections.emptyList();
20442 
20443         final Matrix ba = generateBa();
20444         final double biasX = ba.getElementAtIndex(0);
20445         final double biasY = ba.getElementAtIndex(1);
20446         final double biasZ = ba.getElementAtIndex(2);
20447 
20448         final Acceleration bx = new Acceleration(biasX,
20449                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20450         final Acceleration by = new Acceleration(biasY,
20451                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20452         final Acceleration bz = new Acceleration(biasZ,
20453                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20454 
20455         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20456         final double latitude = Math.toRadians(
20457                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20458         final double longitude = Math.toRadians(
20459                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20460         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20461         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20462         final NEDVelocity nedVelocity = new NEDVelocity();
20463         final ECEFPosition ecefPosition = new ECEFPosition();
20464         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20465         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20466                 ecefPosition, ecefVelocity);
20467         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20468                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20469         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20470 
20471         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
20472                 new KnownBiasAndGravityNormAccelerometerCalibrator(
20473                         gravityNorm, measurements, bx, by, bz);
20474 
20475         // check default values
20476         assertEquals(calibrator.getBiasX(), biasX, 0.0);
20477         assertEquals(calibrator.getBiasY(), biasY, 0.0);
20478         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
20479         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
20480         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20481         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20482         final Acceleration bx2 = new Acceleration(0.0,
20483                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20484         calibrator.getBiasXAsAcceleration(bx2);
20485         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20486         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20487         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
20488         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20489         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20490         final Acceleration by2 = new Acceleration(0.0,
20491                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20492         calibrator.getBiasYAsAcceleration(by2);
20493         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20494         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20495         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
20496         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20497         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20498         final Acceleration bz2 = new Acceleration(0.0,
20499                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20500         calibrator.getBiasZAsAcceleration(bz2);
20501         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20502         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20503         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20504         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20505         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20506         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20507         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20508         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20509         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20510         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20511         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20512         final double[] bias1 = calibrator.getBias();
20513         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20514         final double[] bias2 = new double[3];
20515         calibrator.getBias(bias2);
20516         assertArrayEquals(bias1, bias2, 0.0);
20517         final Matrix b1 = calibrator.getBiasAsMatrix();
20518         assertEquals(b1, ba);
20519         final Matrix b2 = new Matrix(3, 1);
20520         calibrator.getBiasAsMatrix(b2);
20521         assertEquals(b1, b2);
20522         final Matrix ma1 = calibrator.getInitialMa();
20523         assertEquals(ma1, new Matrix(3, 3));
20524         final Matrix ma2 = new Matrix(3, 3);
20525         calibrator.getInitialMa(ma2);
20526         assertEquals(ma1, ma2);
20527         assertSame(calibrator.getMeasurements(), measurements);
20528         assertFalse(calibrator.isCommonAxisUsed());
20529         assertNull(calibrator.getListener());
20530         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20531         assertFalse(calibrator.isReady());
20532         assertFalse(calibrator.isRunning());
20533         assertNull(calibrator.getEstimatedMa());
20534         assertNull(calibrator.getEstimatedSx());
20535         assertNull(calibrator.getEstimatedSy());
20536         assertNull(calibrator.getEstimatedSz());
20537         assertNull(calibrator.getEstimatedMxy());
20538         assertNull(calibrator.getEstimatedMxz());
20539         assertNull(calibrator.getEstimatedMyx());
20540         assertNull(calibrator.getEstimatedMyz());
20541         assertNull(calibrator.getEstimatedMzx());
20542         assertNull(calibrator.getEstimatedMzy());
20543         assertNull(calibrator.getEstimatedCovariance());
20544         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20545         assertNotNull(calibrator.getGroundTruthGravityNorm());
20546         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20547         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20548         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20549                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20550         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20551         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20552         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20553 
20554         // Force IllegalArgumentException
20555         final Acceleration invalidGravityNorm = new Acceleration(
20556                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20557 
20558         calibrator = null;
20559         try {
20560             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
20561                     invalidGravityNorm, measurements, bx, by, bz);
20562             fail("IllegalArgumentException expected but not thrown");
20563         } catch (final IllegalArgumentException ignore) {
20564         }
20565         assertNull(calibrator);
20566     }
20567 
20568     @Test
20569     public void testConstructor176() throws WrongSizeException {
20570         final Collection<StandardDeviationBodyKinematics> measurements =
20571                 Collections.emptyList();
20572 
20573         final Matrix ba = generateBa();
20574         final double biasX = ba.getElementAtIndex(0);
20575         final double biasY = ba.getElementAtIndex(1);
20576         final double biasZ = ba.getElementAtIndex(2);
20577 
20578         final Acceleration bx = new Acceleration(biasX,
20579                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20580         final Acceleration by = new Acceleration(biasY,
20581                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20582         final Acceleration bz = new Acceleration(biasZ,
20583                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20584 
20585         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20586         final double latitude = Math.toRadians(
20587                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20588         final double longitude = Math.toRadians(
20589                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20590         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20591         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20592         final NEDVelocity nedVelocity = new NEDVelocity();
20593         final ECEFPosition ecefPosition = new ECEFPosition();
20594         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20595         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20596                 ecefPosition, ecefVelocity);
20597         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20598                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20599         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20600 
20601         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
20602                 new KnownBiasAndGravityNormAccelerometerCalibrator(
20603                         gravityNorm, measurements,
20604                         bx, by, bz, this);
20605 
20606         // check default values
20607         assertEquals(calibrator.getBiasX(), biasX, 0.0);
20608         assertEquals(calibrator.getBiasY(), biasY, 0.0);
20609         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
20610         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
20611         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20612         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20613         final Acceleration bx2 = new Acceleration(0.0,
20614                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20615         calibrator.getBiasXAsAcceleration(bx2);
20616         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20617         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20618         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
20619         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20620         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20621         final Acceleration by2 = new Acceleration(0.0,
20622                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20623         calibrator.getBiasYAsAcceleration(by2);
20624         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20625         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20626         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
20627         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20628         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20629         final Acceleration bz2 = new Acceleration(0.0,
20630                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20631         calibrator.getBiasZAsAcceleration(bz2);
20632         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20633         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20634         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20635         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20636         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20637         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20638         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20639         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20640         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20641         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20642         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20643         final double[] bias1 = calibrator.getBias();
20644         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20645         final double[] bias2 = new double[3];
20646         calibrator.getBias(bias2);
20647         assertArrayEquals(bias1, bias2, 0.0);
20648         final Matrix b1 = calibrator.getBiasAsMatrix();
20649         assertEquals(b1, ba);
20650         final Matrix b2 = new Matrix(3, 1);
20651         calibrator.getBiasAsMatrix(b2);
20652         assertEquals(b1, b2);
20653         final Matrix ma1 = calibrator.getInitialMa();
20654         assertEquals(ma1, new Matrix(3, 3));
20655         final Matrix ma2 = new Matrix(3, 3);
20656         calibrator.getInitialMa(ma2);
20657         assertEquals(ma1, ma2);
20658         assertSame(calibrator.getMeasurements(), measurements);
20659         assertFalse(calibrator.isCommonAxisUsed());
20660         assertSame(calibrator.getListener(), this);
20661         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20662         assertFalse(calibrator.isReady());
20663         assertFalse(calibrator.isRunning());
20664         assertNull(calibrator.getEstimatedMa());
20665         assertNull(calibrator.getEstimatedSx());
20666         assertNull(calibrator.getEstimatedSy());
20667         assertNull(calibrator.getEstimatedSz());
20668         assertNull(calibrator.getEstimatedMxy());
20669         assertNull(calibrator.getEstimatedMxz());
20670         assertNull(calibrator.getEstimatedMyx());
20671         assertNull(calibrator.getEstimatedMyz());
20672         assertNull(calibrator.getEstimatedMzx());
20673         assertNull(calibrator.getEstimatedMzy());
20674         assertNull(calibrator.getEstimatedCovariance());
20675         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20676         assertNotNull(calibrator.getGroundTruthGravityNorm());
20677         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20678         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20679         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20680                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20681         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20682         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20683         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20684 
20685         // Force IllegalArgumentException
20686         final Acceleration invalidGravityNorm = new Acceleration(
20687                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20688 
20689         calibrator = null;
20690         try {
20691             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
20692                     invalidGravityNorm, measurements,
20693                     bx, by, bz, this);
20694             fail("IllegalArgumentException expected but not thrown");
20695         } catch (final IllegalArgumentException ignore) {
20696         }
20697         assertNull(calibrator);
20698     }
20699 
20700     @Test
20701     public void testConstructor177() throws WrongSizeException {
20702         final Matrix ba = generateBa();
20703         final double biasX = ba.getElementAtIndex(0);
20704         final double biasY = ba.getElementAtIndex(1);
20705         final double biasZ = ba.getElementAtIndex(2);
20706 
20707         final Acceleration bx = new Acceleration(biasX,
20708                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20709         final Acceleration by = new Acceleration(biasY,
20710                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20711         final Acceleration bz = new Acceleration(biasZ,
20712                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20713 
20714         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20715         final double latitude = Math.toRadians(
20716                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20717         final double longitude = Math.toRadians(
20718                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20719         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20720         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20721         final NEDVelocity nedVelocity = new NEDVelocity();
20722         final ECEFPosition ecefPosition = new ECEFPosition();
20723         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20724         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20725                 ecefPosition, ecefVelocity);
20726         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20727                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20728         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20729 
20730         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
20731                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
20732                         true, bx, by, bz);
20733 
20734         // check default values
20735         assertEquals(calibrator.getBiasX(), biasX, 0.0);
20736         assertEquals(calibrator.getBiasY(), biasY, 0.0);
20737         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
20738         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
20739         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20740         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20741         final Acceleration bx2 = new Acceleration(0.0,
20742                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20743         calibrator.getBiasXAsAcceleration(bx2);
20744         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20745         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20746         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
20747         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20748         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20749         final Acceleration by2 = new Acceleration(0.0,
20750                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20751         calibrator.getBiasYAsAcceleration(by2);
20752         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20753         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20754         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
20755         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20756         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20757         final Acceleration bz2 = new Acceleration(0.0,
20758                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20759         calibrator.getBiasZAsAcceleration(bz2);
20760         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20761         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20762         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20763         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20764         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20765         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20766         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20767         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20768         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20769         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20770         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20771         final double[] bias1 = calibrator.getBias();
20772         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20773         final double[] bias2 = new double[3];
20774         calibrator.getBias(bias2);
20775         assertArrayEquals(bias1, bias2, 0.0);
20776         final Matrix b1 = calibrator.getBiasAsMatrix();
20777         assertEquals(b1, ba);
20778         final Matrix b2 = new Matrix(3, 1);
20779         calibrator.getBiasAsMatrix(b2);
20780         assertEquals(b1, b2);
20781         final Matrix ma1 = calibrator.getInitialMa();
20782         assertEquals(ma1, new Matrix(3, 3));
20783         final Matrix ma2 = new Matrix(3, 3);
20784         calibrator.getInitialMa(ma2);
20785         assertEquals(ma1, ma2);
20786         assertNull(calibrator.getMeasurements());
20787         assertTrue(calibrator.isCommonAxisUsed());
20788         assertNull(calibrator.getListener());
20789         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
20790         assertFalse(calibrator.isReady());
20791         assertFalse(calibrator.isRunning());
20792         assertNull(calibrator.getEstimatedMa());
20793         assertNull(calibrator.getEstimatedSx());
20794         assertNull(calibrator.getEstimatedSy());
20795         assertNull(calibrator.getEstimatedSz());
20796         assertNull(calibrator.getEstimatedMxy());
20797         assertNull(calibrator.getEstimatedMxz());
20798         assertNull(calibrator.getEstimatedMyx());
20799         assertNull(calibrator.getEstimatedMyz());
20800         assertNull(calibrator.getEstimatedMzx());
20801         assertNull(calibrator.getEstimatedMzy());
20802         assertNull(calibrator.getEstimatedCovariance());
20803         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20804         assertNotNull(calibrator.getGroundTruthGravityNorm());
20805         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20806         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20807         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20808                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20809         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20810         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20811         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20812 
20813         // Force IllegalArgumentException
20814         final Acceleration invalidGravityNorm = new Acceleration(
20815                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20816 
20817         calibrator = null;
20818         try {
20819             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
20820                     invalidGravityNorm, true,
20821                     bx, by, bz);
20822             fail("IllegalArgumentException expected but not thrown");
20823         } catch (final IllegalArgumentException ignore) {
20824         }
20825         assertNull(calibrator);
20826     }
20827 
20828     @Test
20829     public void testConstructor178() throws WrongSizeException {
20830         final Matrix ba = generateBa();
20831         final double biasX = ba.getElementAtIndex(0);
20832         final double biasY = ba.getElementAtIndex(1);
20833         final double biasZ = ba.getElementAtIndex(2);
20834 
20835         final Acceleration bx = new Acceleration(biasX,
20836                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20837         final Acceleration by = new Acceleration(biasY,
20838                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20839         final Acceleration bz = new Acceleration(biasZ,
20840                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20841 
20842         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20843         final double latitude = Math.toRadians(
20844                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20845         final double longitude = Math.toRadians(
20846                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20847         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20848         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20849         final NEDVelocity nedVelocity = new NEDVelocity();
20850         final ECEFPosition ecefPosition = new ECEFPosition();
20851         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20852         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20853                 ecefPosition, ecefVelocity);
20854         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20855                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20856         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20857 
20858         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
20859                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
20860                         true, bx, by, bz, this);
20861 
20862         // check default values
20863         assertEquals(calibrator.getBiasX(), biasX, 0.0);
20864         assertEquals(calibrator.getBiasY(), biasY, 0.0);
20865         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
20866         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
20867         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20868         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20869         final Acceleration bx2 = new Acceleration(0.0,
20870                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20871         calibrator.getBiasXAsAcceleration(bx2);
20872         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20873         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20874         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
20875         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20876         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20877         final Acceleration by2 = new Acceleration(0.0,
20878                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20879         calibrator.getBiasYAsAcceleration(by2);
20880         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20881         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20882         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
20883         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20884         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20885         final Acceleration bz2 = new Acceleration(0.0,
20886                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20887         calibrator.getBiasZAsAcceleration(bz2);
20888         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20889         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20890         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20891         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20892         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20893         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20894         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20895         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20896         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20897         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20898         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20899         final double[] bias1 = calibrator.getBias();
20900         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20901         final double[] bias2 = new double[3];
20902         calibrator.getBias(bias2);
20903         assertArrayEquals(bias1, bias2, 0.0);
20904         final Matrix b1 = calibrator.getBiasAsMatrix();
20905         assertEquals(b1, ba);
20906         final Matrix b2 = new Matrix(3, 1);
20907         calibrator.getBiasAsMatrix(b2);
20908         assertEquals(b1, b2);
20909         final Matrix ma1 = calibrator.getInitialMa();
20910         assertEquals(ma1, new Matrix(3, 3));
20911         final Matrix ma2 = new Matrix(3, 3);
20912         calibrator.getInitialMa(ma2);
20913         assertEquals(ma1, ma2);
20914         assertNull(calibrator.getMeasurements());
20915         assertTrue(calibrator.isCommonAxisUsed());
20916         assertSame(calibrator.getListener(), this);
20917         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
20918         assertFalse(calibrator.isReady());
20919         assertFalse(calibrator.isRunning());
20920         assertNull(calibrator.getEstimatedMa());
20921         assertNull(calibrator.getEstimatedSx());
20922         assertNull(calibrator.getEstimatedSy());
20923         assertNull(calibrator.getEstimatedSz());
20924         assertNull(calibrator.getEstimatedMxy());
20925         assertNull(calibrator.getEstimatedMxz());
20926         assertNull(calibrator.getEstimatedMyx());
20927         assertNull(calibrator.getEstimatedMyz());
20928         assertNull(calibrator.getEstimatedMzx());
20929         assertNull(calibrator.getEstimatedMzy());
20930         assertNull(calibrator.getEstimatedCovariance());
20931         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20932         assertNotNull(calibrator.getGroundTruthGravityNorm());
20933         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20934         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20935         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20936                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20937         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20938         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20939         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20940 
20941         // Force IllegalArgumentException
20942         final Acceleration invalidGravityNorm = new Acceleration(
20943                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20944 
20945         calibrator = null;
20946         try {
20947             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
20948                     invalidGravityNorm, true,
20949                     bx, by, bz, this);
20950             fail("IllegalArgumentException expected but not thrown");
20951         } catch (final IllegalArgumentException ignore) {
20952         }
20953         assertNull(calibrator);
20954     }
20955 
20956     @Test
20957     public void testConstructor179() throws WrongSizeException {
20958         final Collection<StandardDeviationBodyKinematics> measurements =
20959                 Collections.emptyList();
20960 
20961         final Matrix ba = generateBa();
20962         final double biasX = ba.getElementAtIndex(0);
20963         final double biasY = ba.getElementAtIndex(1);
20964         final double biasZ = ba.getElementAtIndex(2);
20965 
20966         final Acceleration bx = new Acceleration(biasX,
20967                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20968         final Acceleration by = new Acceleration(biasY,
20969                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20970         final Acceleration bz = new Acceleration(biasZ,
20971                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20972 
20973         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20974         final double latitude = Math.toRadians(
20975                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20976         final double longitude = Math.toRadians(
20977                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20978         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20979         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20980         final NEDVelocity nedVelocity = new NEDVelocity();
20981         final ECEFPosition ecefPosition = new ECEFPosition();
20982         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20983         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20984                 ecefPosition, ecefVelocity);
20985         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20986                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20987         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20988 
20989         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
20990                 new KnownBiasAndGravityNormAccelerometerCalibrator(
20991                         gravityNorm, measurements,
20992                         true, bx, by, bz);
20993 
20994         // check default values
20995         assertEquals(calibrator.getBiasX(), biasX, 0.0);
20996         assertEquals(calibrator.getBiasY(), biasY, 0.0);
20997         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
20998         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
20999         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21000         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21001         final Acceleration bx2 = new Acceleration(0.0,
21002                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21003         calibrator.getBiasXAsAcceleration(bx2);
21004         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21005         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21006         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
21007         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21008         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21009         final Acceleration by2 = new Acceleration(0.0,
21010                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21011         calibrator.getBiasYAsAcceleration(by2);
21012         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21013         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21014         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
21015         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21016         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21017         final Acceleration bz2 = new Acceleration(0.0,
21018                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21019         calibrator.getBiasZAsAcceleration(bz2);
21020         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21021         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21022         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21023         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21024         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21025         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21026         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21027         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21028         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21029         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21030         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21031         final double[] bias1 = calibrator.getBias();
21032         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21033         final double[] bias2 = new double[3];
21034         calibrator.getBias(bias2);
21035         assertArrayEquals(bias1, bias2, 0.0);
21036         final Matrix b1 = calibrator.getBiasAsMatrix();
21037         assertEquals(b1, ba);
21038         final Matrix b2 = new Matrix(3, 1);
21039         calibrator.getBiasAsMatrix(b2);
21040         assertEquals(b1, b2);
21041         final Matrix ma1 = calibrator.getInitialMa();
21042         assertEquals(ma1, new Matrix(3, 3));
21043         final Matrix ma2 = new Matrix(3, 3);
21044         calibrator.getInitialMa(ma2);
21045         assertEquals(ma1, ma2);
21046         assertSame(calibrator.getMeasurements(), measurements);
21047         assertTrue(calibrator.isCommonAxisUsed());
21048         assertNull(calibrator.getListener());
21049         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
21050         assertFalse(calibrator.isReady());
21051         assertFalse(calibrator.isRunning());
21052         assertNull(calibrator.getEstimatedMa());
21053         assertNull(calibrator.getEstimatedSx());
21054         assertNull(calibrator.getEstimatedSy());
21055         assertNull(calibrator.getEstimatedSz());
21056         assertNull(calibrator.getEstimatedMxy());
21057         assertNull(calibrator.getEstimatedMxz());
21058         assertNull(calibrator.getEstimatedMyx());
21059         assertNull(calibrator.getEstimatedMyz());
21060         assertNull(calibrator.getEstimatedMzx());
21061         assertNull(calibrator.getEstimatedMzy());
21062         assertNull(calibrator.getEstimatedCovariance());
21063         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21064         assertNotNull(calibrator.getGroundTruthGravityNorm());
21065         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21066         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21067         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21068                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21069         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21070         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21071         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21072 
21073         // Force IllegalArgumentException
21074         final Acceleration invalidGravityNorm = new Acceleration(
21075                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21076 
21077         calibrator = null;
21078         try {
21079             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
21080                     invalidGravityNorm, measurements,
21081                     true, bx, by, bz);
21082             fail("IllegalArgumentException expected but not thrown");
21083         } catch (final IllegalArgumentException ignore) {
21084         }
21085         assertNull(calibrator);
21086     }
21087 
21088     @Test
21089     public void testConstructor180() throws WrongSizeException {
21090         final Collection<StandardDeviationBodyKinematics> measurements =
21091                 Collections.emptyList();
21092 
21093         final Matrix ba = generateBa();
21094         final double biasX = ba.getElementAtIndex(0);
21095         final double biasY = ba.getElementAtIndex(1);
21096         final double biasZ = ba.getElementAtIndex(2);
21097 
21098         final Acceleration bx = new Acceleration(biasX,
21099                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21100         final Acceleration by = new Acceleration(biasY,
21101                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21102         final Acceleration bz = new Acceleration(biasZ,
21103                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21104 
21105         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21106         final double latitude = Math.toRadians(
21107                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21108         final double longitude = Math.toRadians(
21109                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21110         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21111         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21112         final NEDVelocity nedVelocity = new NEDVelocity();
21113         final ECEFPosition ecefPosition = new ECEFPosition();
21114         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21115         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21116                 ecefPosition, ecefVelocity);
21117         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21118                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21119         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21120 
21121         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
21122                 new KnownBiasAndGravityNormAccelerometerCalibrator(
21123                         gravityNorm, measurements,
21124                         true, bx, by, bz, this);
21125 
21126         // check default values
21127         assertEquals(calibrator.getBiasX(), biasX, 0.0);
21128         assertEquals(calibrator.getBiasY(), biasY, 0.0);
21129         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
21130         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
21131         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21132         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21133         final Acceleration bx2 = new Acceleration(0.0,
21134                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21135         calibrator.getBiasXAsAcceleration(bx2);
21136         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21137         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21138         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
21139         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21140         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21141         final Acceleration by2 = new Acceleration(0.0,
21142                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21143         calibrator.getBiasYAsAcceleration(by2);
21144         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21145         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21146         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
21147         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21148         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21149         final Acceleration bz2 = new Acceleration(0.0,
21150                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21151         calibrator.getBiasZAsAcceleration(bz2);
21152         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21153         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21154         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21155         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21156         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21157         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21158         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21159         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21160         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21161         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21162         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21163         final double[] bias1 = calibrator.getBias();
21164         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21165         final double[] bias2 = new double[3];
21166         calibrator.getBias(bias2);
21167         assertArrayEquals(bias1, bias2, 0.0);
21168         final Matrix b1 = calibrator.getBiasAsMatrix();
21169         assertEquals(b1, ba);
21170         final Matrix b2 = new Matrix(3, 1);
21171         calibrator.getBiasAsMatrix(b2);
21172         assertEquals(b1, b2);
21173         final Matrix ma1 = calibrator.getInitialMa();
21174         assertEquals(ma1, new Matrix(3, 3));
21175         final Matrix ma2 = new Matrix(3, 3);
21176         calibrator.getInitialMa(ma2);
21177         assertEquals(ma1, ma2);
21178         assertSame(calibrator.getMeasurements(), measurements);
21179         assertTrue(calibrator.isCommonAxisUsed());
21180         assertSame(calibrator.getListener(), this);
21181         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
21182         assertFalse(calibrator.isReady());
21183         assertFalse(calibrator.isRunning());
21184         assertNull(calibrator.getEstimatedMa());
21185         assertNull(calibrator.getEstimatedSx());
21186         assertNull(calibrator.getEstimatedSy());
21187         assertNull(calibrator.getEstimatedSz());
21188         assertNull(calibrator.getEstimatedMxy());
21189         assertNull(calibrator.getEstimatedMxz());
21190         assertNull(calibrator.getEstimatedMyx());
21191         assertNull(calibrator.getEstimatedMyz());
21192         assertNull(calibrator.getEstimatedMzx());
21193         assertNull(calibrator.getEstimatedMzy());
21194         assertNull(calibrator.getEstimatedCovariance());
21195         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21196         assertNotNull(calibrator.getGroundTruthGravityNorm());
21197         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21198         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21199         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21200                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21201         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21202         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21203         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21204 
21205         // Force IllegalArgumentException
21206         final Acceleration invalidGravityNorm = new Acceleration(
21207                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21208 
21209         calibrator = null;
21210         try {
21211             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
21212                     invalidGravityNorm, measurements, true,
21213                     bx, by, bz, this);
21214             fail("IllegalArgumentException expected but not thrown");
21215         } catch (final IllegalArgumentException ignore) {
21216         }
21217         assertNull(calibrator);
21218     }
21219 
21220     @Test
21221     public void testConstructor181() throws WrongSizeException {
21222         final Matrix ba = generateBa();
21223         final double biasX = ba.getElementAtIndex(0);
21224         final double biasY = ba.getElementAtIndex(1);
21225         final double biasZ = ba.getElementAtIndex(2);
21226 
21227         final Matrix ma = generateMaCommonAxis();
21228         final double sx = ma.getElementAt(0, 0);
21229         final double sy = ma.getElementAt(1, 1);
21230         final double sz = ma.getElementAt(2, 2);
21231 
21232         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21233         final double latitude = Math.toRadians(
21234                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21235         final double longitude = Math.toRadians(
21236                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21237         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21238         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21239         final NEDVelocity nedVelocity = new NEDVelocity();
21240         final ECEFPosition ecefPosition = new ECEFPosition();
21241         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21242         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21243                 ecefPosition, ecefVelocity);
21244         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21245                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21246         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21247 
21248         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
21249                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
21250                         biasX, biasY, biasZ, sx, sy, sz);
21251 
21252         // check default values
21253         assertEquals(calibrator.getBiasX(), biasX, 0.0);
21254         assertEquals(calibrator.getBiasY(), biasY, 0.0);
21255         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
21256         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
21257         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21258         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21259         final Acceleration bx2 = new Acceleration(0.0,
21260                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21261         calibrator.getBiasXAsAcceleration(bx2);
21262         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21263         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21264         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
21265         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21266         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21267         final Acceleration by2 = new Acceleration(0.0,
21268                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21269         calibrator.getBiasYAsAcceleration(by2);
21270         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21271         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21272         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
21273         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21274         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21275         final Acceleration bz2 = new Acceleration(0.0,
21276                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21277         calibrator.getBiasZAsAcceleration(bz2);
21278         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21279         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21280         assertEquals(calibrator.getInitialSx(), sx, 0.0);
21281         assertEquals(calibrator.getInitialSy(), sy, 0.0);
21282         assertEquals(calibrator.getInitialSz(), sz, 0.0);
21283         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21284         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21285         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21286         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21287         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21288         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21289         final double[] bias1 = calibrator.getBias();
21290         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21291         final double[] bias2 = new double[3];
21292         calibrator.getBias(bias2);
21293         assertArrayEquals(bias1, bias2, 0.0);
21294         final Matrix b1 = calibrator.getBiasAsMatrix();
21295         assertEquals(b1, ba);
21296         final Matrix b2 = new Matrix(3, 1);
21297         calibrator.getBiasAsMatrix(b2);
21298         assertEquals(b1, b2);
21299         final Matrix ma1 = calibrator.getInitialMa();
21300         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21301         final Matrix ma2 = new Matrix(3, 3);
21302         calibrator.getInitialMa(ma2);
21303         assertEquals(ma1, ma2);
21304         assertNull(calibrator.getMeasurements());
21305         assertFalse(calibrator.isCommonAxisUsed());
21306         assertNull(calibrator.getListener());
21307         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21308         assertFalse(calibrator.isReady());
21309         assertFalse(calibrator.isRunning());
21310         assertNull(calibrator.getEstimatedMa());
21311         assertNull(calibrator.getEstimatedSx());
21312         assertNull(calibrator.getEstimatedSy());
21313         assertNull(calibrator.getEstimatedSz());
21314         assertNull(calibrator.getEstimatedMxy());
21315         assertNull(calibrator.getEstimatedMxz());
21316         assertNull(calibrator.getEstimatedMyx());
21317         assertNull(calibrator.getEstimatedMyz());
21318         assertNull(calibrator.getEstimatedMzx());
21319         assertNull(calibrator.getEstimatedMzy());
21320         assertNull(calibrator.getEstimatedCovariance());
21321         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21322         assertNotNull(calibrator.getGroundTruthGravityNorm());
21323         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21324         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21325         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21326                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21327         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21328         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21329         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21330 
21331         // Force IllegalArgumentException
21332         final Acceleration invalidGravityNorm = new Acceleration(
21333                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21334 
21335         calibrator = null;
21336         try {
21337             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
21338                     invalidGravityNorm, biasX, biasY, biasZ, sx, sy, sz);
21339             fail("IllegalArgumentException expected but not thrown");
21340         } catch (final IllegalArgumentException ignore) {
21341         }
21342         assertNull(calibrator);
21343     }
21344 
21345     @Test
21346     public void testConstructor182() throws WrongSizeException {
21347         final Collection<StandardDeviationBodyKinematics> measurements =
21348                 Collections.emptyList();
21349 
21350         final Matrix ba = generateBa();
21351         final double biasX = ba.getElementAtIndex(0);
21352         final double biasY = ba.getElementAtIndex(1);
21353         final double biasZ = ba.getElementAtIndex(2);
21354 
21355         final Matrix ma = generateMaCommonAxis();
21356         final double sx = ma.getElementAt(0, 0);
21357         final double sy = ma.getElementAt(1, 1);
21358         final double sz = ma.getElementAt(2, 2);
21359 
21360         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21361         final double latitude = Math.toRadians(
21362                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21363         final double longitude = Math.toRadians(
21364                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21365         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21366         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21367         final NEDVelocity nedVelocity = new NEDVelocity();
21368         final ECEFPosition ecefPosition = new ECEFPosition();
21369         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21370         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21371                 ecefPosition, ecefVelocity);
21372         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21373                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21374         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21375 
21376         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
21377                 new KnownBiasAndGravityNormAccelerometerCalibrator(
21378                         gravityNorm, measurements,
21379                         biasX, biasY, biasZ, sx, sy, sz);
21380 
21381         // check default values
21382         assertEquals(calibrator.getBiasX(), biasX, 0.0);
21383         assertEquals(calibrator.getBiasY(), biasY, 0.0);
21384         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
21385         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
21386         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21387         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21388         final Acceleration bx2 = new Acceleration(0.0,
21389                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21390         calibrator.getBiasXAsAcceleration(bx2);
21391         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21392         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21393         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
21394         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21395         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21396         final Acceleration by2 = new Acceleration(0.0,
21397                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21398         calibrator.getBiasYAsAcceleration(by2);
21399         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21400         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21401         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
21402         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21403         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21404         final Acceleration bz2 = new Acceleration(0.0,
21405                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21406         calibrator.getBiasZAsAcceleration(bz2);
21407         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21408         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21409         assertEquals(calibrator.getInitialSx(), sx, 0.0);
21410         assertEquals(calibrator.getInitialSy(), sy, 0.0);
21411         assertEquals(calibrator.getInitialSz(), sz, 0.0);
21412         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21413         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21414         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21415         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21416         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21417         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21418         final double[] bias1 = calibrator.getBias();
21419         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21420         final double[] bias2 = new double[3];
21421         calibrator.getBias(bias2);
21422         assertArrayEquals(bias1, bias2, 0.0);
21423         final Matrix b1 = calibrator.getBiasAsMatrix();
21424         assertEquals(b1, ba);
21425         final Matrix b2 = new Matrix(3, 1);
21426         calibrator.getBiasAsMatrix(b2);
21427         assertEquals(b1, b2);
21428         final Matrix ma1 = calibrator.getInitialMa();
21429         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21430         final Matrix ma2 = new Matrix(3, 3);
21431         calibrator.getInitialMa(ma2);
21432         assertEquals(ma1, ma2);
21433         assertSame(calibrator.getMeasurements(), measurements);
21434         assertFalse(calibrator.isCommonAxisUsed());
21435         assertNull(calibrator.getListener());
21436         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21437         assertFalse(calibrator.isReady());
21438         assertFalse(calibrator.isRunning());
21439         assertNull(calibrator.getEstimatedMa());
21440         assertNull(calibrator.getEstimatedSx());
21441         assertNull(calibrator.getEstimatedSy());
21442         assertNull(calibrator.getEstimatedSz());
21443         assertNull(calibrator.getEstimatedMxy());
21444         assertNull(calibrator.getEstimatedMxz());
21445         assertNull(calibrator.getEstimatedMyx());
21446         assertNull(calibrator.getEstimatedMyz());
21447         assertNull(calibrator.getEstimatedMzx());
21448         assertNull(calibrator.getEstimatedMzy());
21449         assertNull(calibrator.getEstimatedCovariance());
21450         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21451         assertNotNull(calibrator.getGroundTruthGravityNorm());
21452         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21453         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21454         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21455                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21456         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21457         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21458         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21459 
21460         // Force IllegalArgumentException
21461         final Acceleration invalidGravityNorm = new Acceleration(
21462                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21463 
21464         calibrator = null;
21465         try {
21466             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
21467                     invalidGravityNorm, measurements,
21468                     biasX, biasY, biasZ, sx, sy, sz);
21469             fail("IllegalArgumentException expected but not thrown");
21470         } catch (final IllegalArgumentException ignore) {
21471         }
21472         assertNull(calibrator);
21473     }
21474 
21475     @Test
21476     public void testConstructor183() throws WrongSizeException {
21477         final Collection<StandardDeviationBodyKinematics> measurements =
21478                 Collections.emptyList();
21479 
21480         final Matrix ba = generateBa();
21481         final double biasX = ba.getElementAtIndex(0);
21482         final double biasY = ba.getElementAtIndex(1);
21483         final double biasZ = ba.getElementAtIndex(2);
21484 
21485         final Matrix ma = generateMaCommonAxis();
21486         final double sx = ma.getElementAt(0, 0);
21487         final double sy = ma.getElementAt(1, 1);
21488         final double sz = ma.getElementAt(2, 2);
21489 
21490         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21491         final double latitude = Math.toRadians(
21492                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21493         final double longitude = Math.toRadians(
21494                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21495         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21496         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21497         final NEDVelocity nedVelocity = new NEDVelocity();
21498         final ECEFPosition ecefPosition = new ECEFPosition();
21499         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21500         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21501                 ecefPosition, ecefVelocity);
21502         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21503                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21504         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21505 
21506         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
21507                 new KnownBiasAndGravityNormAccelerometerCalibrator(
21508                         gravityNorm, measurements,
21509                         biasX, biasY, biasZ, sx, sy, sz, this);
21510 
21511         // check default values
21512         assertEquals(calibrator.getBiasX(), biasX, 0.0);
21513         assertEquals(calibrator.getBiasY(), biasY, 0.0);
21514         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
21515         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
21516         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21517         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21518         final Acceleration bx2 = new Acceleration(0.0,
21519                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21520         calibrator.getBiasXAsAcceleration(bx2);
21521         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21522         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21523         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
21524         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21525         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21526         final Acceleration by2 = new Acceleration(0.0,
21527                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21528         calibrator.getBiasYAsAcceleration(by2);
21529         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21530         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21531         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
21532         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21533         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21534         final Acceleration bz2 = new Acceleration(0.0,
21535                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21536         calibrator.getBiasZAsAcceleration(bz2);
21537         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21538         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21539         assertEquals(calibrator.getInitialSx(), sx, 0.0);
21540         assertEquals(calibrator.getInitialSy(), sy, 0.0);
21541         assertEquals(calibrator.getInitialSz(), sz, 0.0);
21542         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21543         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21544         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21545         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21546         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21547         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21548         final double[] bias1 = calibrator.getBias();
21549         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21550         final double[] bias2 = new double[3];
21551         calibrator.getBias(bias2);
21552         assertArrayEquals(bias1, bias2, 0.0);
21553         final Matrix b1 = calibrator.getBiasAsMatrix();
21554         assertEquals(b1, ba);
21555         final Matrix b2 = new Matrix(3, 1);
21556         calibrator.getBiasAsMatrix(b2);
21557         assertEquals(b1, b2);
21558         final Matrix ma1 = calibrator.getInitialMa();
21559         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21560         final Matrix ma2 = new Matrix(3, 3);
21561         calibrator.getInitialMa(ma2);
21562         assertEquals(ma1, ma2);
21563         assertSame(calibrator.getMeasurements(), measurements);
21564         assertFalse(calibrator.isCommonAxisUsed());
21565         assertSame(calibrator.getListener(), this);
21566         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21567         assertFalse(calibrator.isReady());
21568         assertFalse(calibrator.isRunning());
21569         assertNull(calibrator.getEstimatedMa());
21570         assertNull(calibrator.getEstimatedSx());
21571         assertNull(calibrator.getEstimatedSy());
21572         assertNull(calibrator.getEstimatedSz());
21573         assertNull(calibrator.getEstimatedMxy());
21574         assertNull(calibrator.getEstimatedMxz());
21575         assertNull(calibrator.getEstimatedMyx());
21576         assertNull(calibrator.getEstimatedMyz());
21577         assertNull(calibrator.getEstimatedMzx());
21578         assertNull(calibrator.getEstimatedMzy());
21579         assertNull(calibrator.getEstimatedCovariance());
21580         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21581         assertNotNull(calibrator.getGroundTruthGravityNorm());
21582         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21583         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21584         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21585                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21586         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21587         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21588         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21589 
21590         // Force IllegalArgumentException
21591         final Acceleration invalidGravityNorm = new Acceleration(
21592                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21593 
21594         calibrator = null;
21595         try {
21596             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
21597                     invalidGravityNorm, measurements,
21598                     biasX, biasY, biasZ, sx, sy, sz, this);
21599             fail("IllegalArgumentException expected but not thrown");
21600         } catch (final IllegalArgumentException ignore) {
21601         }
21602         assertNull(calibrator);
21603     }
21604 
21605     @Test
21606     public void testConstructor184() throws WrongSizeException {
21607         final Matrix ba = generateBa();
21608         final double biasX = ba.getElementAtIndex(0);
21609         final double biasY = ba.getElementAtIndex(1);
21610         final double biasZ = ba.getElementAtIndex(2);
21611 
21612         final Matrix ma = generateMaCommonAxis();
21613         final double sx = ma.getElementAt(0, 0);
21614         final double sy = ma.getElementAt(1, 1);
21615         final double sz = ma.getElementAt(2, 2);
21616 
21617         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21618         final double latitude = Math.toRadians(
21619                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21620         final double longitude = Math.toRadians(
21621                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21622         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21623         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21624         final NEDVelocity nedVelocity = new NEDVelocity();
21625         final ECEFPosition ecefPosition = new ECEFPosition();
21626         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21627         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21628                 ecefPosition, ecefVelocity);
21629         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21630                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21631         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21632 
21633         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
21634                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
21635                         true, biasX, biasY, biasZ, sx, sy, sz);
21636 
21637         // check default values
21638         assertEquals(calibrator.getBiasX(), biasX, 0.0);
21639         assertEquals(calibrator.getBiasY(), biasY, 0.0);
21640         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
21641         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
21642         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21643         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21644         final Acceleration bx2 = new Acceleration(0.0,
21645                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21646         calibrator.getBiasXAsAcceleration(bx2);
21647         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21648         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21649         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
21650         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21651         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21652         final Acceleration by2 = new Acceleration(0.0,
21653                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21654         calibrator.getBiasYAsAcceleration(by2);
21655         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21656         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21657         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
21658         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21659         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21660         final Acceleration bz2 = new Acceleration(0.0,
21661                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21662         calibrator.getBiasZAsAcceleration(bz2);
21663         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21664         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21665         assertEquals(calibrator.getInitialSx(), sx, 0.0);
21666         assertEquals(calibrator.getInitialSy(), sy, 0.0);
21667         assertEquals(calibrator.getInitialSz(), sz, 0.0);
21668         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21669         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21670         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21671         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21672         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21673         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21674         final double[] bias1 = calibrator.getBias();
21675         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21676         final double[] bias2 = new double[3];
21677         calibrator.getBias(bias2);
21678         assertArrayEquals(bias1, bias2, 0.0);
21679         final Matrix b1 = calibrator.getBiasAsMatrix();
21680         assertEquals(b1, ba);
21681         final Matrix b2 = new Matrix(3, 1);
21682         calibrator.getBiasAsMatrix(b2);
21683         assertEquals(b1, b2);
21684         final Matrix ma1 = calibrator.getInitialMa();
21685         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21686         final Matrix ma2 = new Matrix(3, 3);
21687         calibrator.getInitialMa(ma2);
21688         assertEquals(ma1, ma2);
21689         assertNull(calibrator.getMeasurements());
21690         assertTrue(calibrator.isCommonAxisUsed());
21691         assertNull(calibrator.getListener());
21692         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
21693         assertFalse(calibrator.isReady());
21694         assertFalse(calibrator.isRunning());
21695         assertNull(calibrator.getEstimatedMa());
21696         assertNull(calibrator.getEstimatedSx());
21697         assertNull(calibrator.getEstimatedSy());
21698         assertNull(calibrator.getEstimatedSz());
21699         assertNull(calibrator.getEstimatedMxy());
21700         assertNull(calibrator.getEstimatedMxz());
21701         assertNull(calibrator.getEstimatedMyx());
21702         assertNull(calibrator.getEstimatedMyz());
21703         assertNull(calibrator.getEstimatedMzx());
21704         assertNull(calibrator.getEstimatedMzy());
21705         assertNull(calibrator.getEstimatedCovariance());
21706         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21707         assertNotNull(calibrator.getGroundTruthGravityNorm());
21708         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21709         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21710         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21711                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21712         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21713         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21714         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21715 
21716         // Force IllegalArgumentException
21717         final Acceleration invalidGravityNorm = new Acceleration(
21718                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21719 
21720         calibrator = null;
21721         try {
21722             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
21723                     invalidGravityNorm, true, biasX, biasY, biasZ,
21724                     sx, sy, sz);
21725             fail("IllegalArgumentException expected but not thrown");
21726         } catch (final IllegalArgumentException ignore) {
21727         }
21728         assertNull(calibrator);
21729     }
21730 
21731     @Test
21732     public void testConstructor185() throws WrongSizeException {
21733         final Matrix ba = generateBa();
21734         final double biasX = ba.getElementAtIndex(0);
21735         final double biasY = ba.getElementAtIndex(1);
21736         final double biasZ = ba.getElementAtIndex(2);
21737 
21738         final Matrix ma = generateMaCommonAxis();
21739         final double sx = ma.getElementAt(0, 0);
21740         final double sy = ma.getElementAt(1, 1);
21741         final double sz = ma.getElementAt(2, 2);
21742 
21743         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21744         final double latitude = Math.toRadians(
21745                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21746         final double longitude = Math.toRadians(
21747                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21748         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21749         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21750         final NEDVelocity nedVelocity = new NEDVelocity();
21751         final ECEFPosition ecefPosition = new ECEFPosition();
21752         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21753         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21754                 ecefPosition, ecefVelocity);
21755         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21756                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21757         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21758 
21759         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
21760                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
21761                         true, biasX, biasY, biasZ, sx, sy, sz,
21762                         this);
21763 
21764         // check default values
21765         assertEquals(calibrator.getBiasX(), biasX, 0.0);
21766         assertEquals(calibrator.getBiasY(), biasY, 0.0);
21767         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
21768         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
21769         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21770         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21771         final Acceleration bx2 = new Acceleration(0.0,
21772                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21773         calibrator.getBiasXAsAcceleration(bx2);
21774         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21775         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21776         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
21777         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21778         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21779         final Acceleration by2 = new Acceleration(0.0,
21780                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21781         calibrator.getBiasYAsAcceleration(by2);
21782         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21783         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21784         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
21785         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21786         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21787         final Acceleration bz2 = new Acceleration(0.0,
21788                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21789         calibrator.getBiasZAsAcceleration(bz2);
21790         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21791         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21792         assertEquals(calibrator.getInitialSx(), sx, 0.0);
21793         assertEquals(calibrator.getInitialSy(), sy, 0.0);
21794         assertEquals(calibrator.getInitialSz(), sz, 0.0);
21795         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21796         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21797         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21798         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21799         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21800         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21801         final double[] bias1 = calibrator.getBias();
21802         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21803         final double[] bias2 = new double[3];
21804         calibrator.getBias(bias2);
21805         assertArrayEquals(bias1, bias2, 0.0);
21806         final Matrix b1 = calibrator.getBiasAsMatrix();
21807         assertEquals(b1, ba);
21808         final Matrix b2 = new Matrix(3, 1);
21809         calibrator.getBiasAsMatrix(b2);
21810         assertEquals(b1, b2);
21811         final Matrix ma1 = calibrator.getInitialMa();
21812         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21813         final Matrix ma2 = new Matrix(3, 3);
21814         calibrator.getInitialMa(ma2);
21815         assertEquals(ma1, ma2);
21816         assertNull(calibrator.getMeasurements());
21817         assertTrue(calibrator.isCommonAxisUsed());
21818         assertSame(calibrator.getListener(), this);
21819         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
21820         assertFalse(calibrator.isReady());
21821         assertFalse(calibrator.isRunning());
21822         assertNull(calibrator.getEstimatedMa());
21823         assertNull(calibrator.getEstimatedSx());
21824         assertNull(calibrator.getEstimatedSy());
21825         assertNull(calibrator.getEstimatedSz());
21826         assertNull(calibrator.getEstimatedMxy());
21827         assertNull(calibrator.getEstimatedMxz());
21828         assertNull(calibrator.getEstimatedMyx());
21829         assertNull(calibrator.getEstimatedMyz());
21830         assertNull(calibrator.getEstimatedMzx());
21831         assertNull(calibrator.getEstimatedMzy());
21832         assertNull(calibrator.getEstimatedCovariance());
21833         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21834         assertNotNull(calibrator.getGroundTruthGravityNorm());
21835         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21836         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21837         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21838                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21839         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21840         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21841         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21842 
21843         // Force IllegalArgumentException
21844         final Acceleration invalidGravityNorm = new Acceleration(
21845                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21846 
21847         calibrator = null;
21848         try {
21849             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
21850                     invalidGravityNorm, true,
21851                     biasX, biasY, biasZ,
21852                     sx, sy, sz, this);
21853             fail("IllegalArgumentException expected but not thrown");
21854         } catch (final IllegalArgumentException ignore) {
21855         }
21856         assertNull(calibrator);
21857     }
21858 
21859     @Test
21860     public void testConstructor186() throws WrongSizeException {
21861         final Collection<StandardDeviationBodyKinematics> measurements =
21862                 Collections.emptyList();
21863 
21864         final Matrix ba = generateBa();
21865         final double biasX = ba.getElementAtIndex(0);
21866         final double biasY = ba.getElementAtIndex(1);
21867         final double biasZ = ba.getElementAtIndex(2);
21868 
21869         final Matrix ma = generateMaCommonAxis();
21870         final double sx = ma.getElementAt(0, 0);
21871         final double sy = ma.getElementAt(1, 1);
21872         final double sz = ma.getElementAt(2, 2);
21873 
21874         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21875         final double latitude = Math.toRadians(
21876                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21877         final double longitude = Math.toRadians(
21878                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21879         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21880         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21881         final NEDVelocity nedVelocity = new NEDVelocity();
21882         final ECEFPosition ecefPosition = new ECEFPosition();
21883         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21884         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21885                 ecefPosition, ecefVelocity);
21886         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21887                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21888         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21889 
21890         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
21891                 new KnownBiasAndGravityNormAccelerometerCalibrator(
21892                         gravityNorm, measurements,
21893                         true, biasX, biasY, biasZ, sx, sy, sz);
21894 
21895         // check default values
21896         assertEquals(calibrator.getBiasX(), biasX, 0.0);
21897         assertEquals(calibrator.getBiasY(), biasY, 0.0);
21898         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
21899         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
21900         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21901         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21902         final Acceleration bx2 = new Acceleration(0.0,
21903                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21904         calibrator.getBiasXAsAcceleration(bx2);
21905         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21906         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21907         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
21908         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21909         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21910         final Acceleration by2 = new Acceleration(0.0,
21911                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21912         calibrator.getBiasYAsAcceleration(by2);
21913         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21914         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21915         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
21916         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21917         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21918         final Acceleration bz2 = new Acceleration(0.0,
21919                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21920         calibrator.getBiasZAsAcceleration(bz2);
21921         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21922         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21923         assertEquals(calibrator.getInitialSx(), sx, 0.0);
21924         assertEquals(calibrator.getInitialSy(), sy, 0.0);
21925         assertEquals(calibrator.getInitialSz(), sz, 0.0);
21926         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21927         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21928         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21929         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21930         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21931         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21932         final double[] bias1 = calibrator.getBias();
21933         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21934         final double[] bias2 = new double[3];
21935         calibrator.getBias(bias2);
21936         assertArrayEquals(bias1, bias2, 0.0);
21937         final Matrix b1 = calibrator.getBiasAsMatrix();
21938         assertEquals(b1, ba);
21939         final Matrix b2 = new Matrix(3, 1);
21940         calibrator.getBiasAsMatrix(b2);
21941         assertEquals(b1, b2);
21942         final Matrix ma1 = calibrator.getInitialMa();
21943         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21944         final Matrix ma2 = new Matrix(3, 3);
21945         calibrator.getInitialMa(ma2);
21946         assertEquals(ma1, ma2);
21947         assertSame(calibrator.getMeasurements(), measurements);
21948         assertTrue(calibrator.isCommonAxisUsed());
21949         assertNull(calibrator.getListener());
21950         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
21951         assertFalse(calibrator.isReady());
21952         assertFalse(calibrator.isRunning());
21953         assertNull(calibrator.getEstimatedMa());
21954         assertNull(calibrator.getEstimatedSx());
21955         assertNull(calibrator.getEstimatedSy());
21956         assertNull(calibrator.getEstimatedSz());
21957         assertNull(calibrator.getEstimatedMxy());
21958         assertNull(calibrator.getEstimatedMxz());
21959         assertNull(calibrator.getEstimatedMyx());
21960         assertNull(calibrator.getEstimatedMyz());
21961         assertNull(calibrator.getEstimatedMzx());
21962         assertNull(calibrator.getEstimatedMzy());
21963         assertNull(calibrator.getEstimatedCovariance());
21964         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21965         assertNotNull(calibrator.getGroundTruthGravityNorm());
21966         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21967         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21968         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21969                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21970         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21971         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21972         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21973 
21974         // Force IllegalArgumentException
21975         final Acceleration invalidGravityNorm = new Acceleration(
21976                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21977 
21978         calibrator = null;
21979         try {
21980             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
21981                     invalidGravityNorm, measurements,
21982                     true, biasX, biasY, biasZ, sx, sy, sz);
21983             fail("IllegalArgumentException expected but not thrown");
21984         } catch (final IllegalArgumentException ignore) {
21985         }
21986         assertNull(calibrator);
21987     }
21988 
21989     @Test
21990     public void testConstructor187() throws WrongSizeException {
21991         final Collection<StandardDeviationBodyKinematics> measurements =
21992                 Collections.emptyList();
21993 
21994         final Matrix ba = generateBa();
21995         final double biasX = ba.getElementAtIndex(0);
21996         final double biasY = ba.getElementAtIndex(1);
21997         final double biasZ = ba.getElementAtIndex(2);
21998 
21999         final Matrix ma = generateMaCommonAxis();
22000         final double sx = ma.getElementAt(0, 0);
22001         final double sy = ma.getElementAt(1, 1);
22002         final double sz = ma.getElementAt(2, 2);
22003 
22004         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22005         final double latitude = Math.toRadians(
22006                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22007         final double longitude = Math.toRadians(
22008                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22009         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22010         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22011         final NEDVelocity nedVelocity = new NEDVelocity();
22012         final ECEFPosition ecefPosition = new ECEFPosition();
22013         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22014         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22015                 ecefPosition, ecefVelocity);
22016         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22017                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22018         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22019 
22020         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
22021                 new KnownBiasAndGravityNormAccelerometerCalibrator(
22022                         gravityNorm, measurements,
22023                         true, biasX, biasY, biasZ, sx, sy, sz,
22024                         this);
22025 
22026         // check default values
22027         assertEquals(calibrator.getBiasX(), biasX, 0.0);
22028         assertEquals(calibrator.getBiasY(), biasY, 0.0);
22029         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
22030         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
22031         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22032         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22033         final Acceleration bx2 = new Acceleration(0.0,
22034                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22035         calibrator.getBiasXAsAcceleration(bx2);
22036         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22037         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22038         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
22039         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22040         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22041         final Acceleration by2 = new Acceleration(0.0,
22042                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22043         calibrator.getBiasYAsAcceleration(by2);
22044         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22045         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22046         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
22047         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22048         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22049         final Acceleration bz2 = new Acceleration(0.0,
22050                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22051         calibrator.getBiasZAsAcceleration(bz2);
22052         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22053         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22054         assertEquals(calibrator.getInitialSx(), sx, 0.0);
22055         assertEquals(calibrator.getInitialSy(), sy, 0.0);
22056         assertEquals(calibrator.getInitialSz(), sz, 0.0);
22057         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22058         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22059         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22060         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22061         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22062         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22063         final double[] bias1 = calibrator.getBias();
22064         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22065         final double[] bias2 = new double[3];
22066         calibrator.getBias(bias2);
22067         assertArrayEquals(bias1, bias2, 0.0);
22068         final Matrix b1 = calibrator.getBiasAsMatrix();
22069         assertEquals(b1, ba);
22070         final Matrix b2 = new Matrix(3, 1);
22071         calibrator.getBiasAsMatrix(b2);
22072         assertEquals(b1, b2);
22073         final Matrix ma1 = calibrator.getInitialMa();
22074         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22075         final Matrix ma2 = new Matrix(3, 3);
22076         calibrator.getInitialMa(ma2);
22077         assertEquals(ma1, ma2);
22078         assertSame(calibrator.getMeasurements(), measurements);
22079         assertTrue(calibrator.isCommonAxisUsed());
22080         assertSame(calibrator.getListener(), this);
22081         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
22082         assertFalse(calibrator.isReady());
22083         assertFalse(calibrator.isRunning());
22084         assertNull(calibrator.getEstimatedMa());
22085         assertNull(calibrator.getEstimatedSx());
22086         assertNull(calibrator.getEstimatedSy());
22087         assertNull(calibrator.getEstimatedSz());
22088         assertNull(calibrator.getEstimatedMxy());
22089         assertNull(calibrator.getEstimatedMxz());
22090         assertNull(calibrator.getEstimatedMyx());
22091         assertNull(calibrator.getEstimatedMyz());
22092         assertNull(calibrator.getEstimatedMzx());
22093         assertNull(calibrator.getEstimatedMzy());
22094         assertNull(calibrator.getEstimatedCovariance());
22095         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22096         assertNotNull(calibrator.getGroundTruthGravityNorm());
22097         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22098         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22099         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22100                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22101         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22102         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22103         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22104 
22105         // Force IllegalArgumentException
22106         final Acceleration invalidGravityNorm = new Acceleration(
22107                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22108 
22109         calibrator = null;
22110         try {
22111             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
22112                     invalidGravityNorm, measurements,
22113                     true, biasX, biasY, biasZ, sx, sy, sz,
22114                     this);
22115             fail("IllegalArgumentException expected but not thrown");
22116         } catch (final IllegalArgumentException ignore) {
22117         }
22118         assertNull(calibrator);
22119     }
22120 
22121     @Test
22122     public void testConstructor188() throws WrongSizeException {
22123         final Matrix ba = generateBa();
22124         final double biasX = ba.getElementAtIndex(0);
22125         final double biasY = ba.getElementAtIndex(1);
22126         final double biasZ = ba.getElementAtIndex(2);
22127 
22128         final Acceleration bx = new Acceleration(biasX,
22129                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22130         final Acceleration by = new Acceleration(biasY,
22131                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22132         final Acceleration bz = new Acceleration(biasZ,
22133                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22134 
22135         final Matrix ma = generateMaCommonAxis();
22136         final double sx = ma.getElementAt(0, 0);
22137         final double sy = ma.getElementAt(1, 1);
22138         final double sz = ma.getElementAt(2, 2);
22139 
22140         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22141         final double latitude = Math.toRadians(
22142                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22143         final double longitude = Math.toRadians(
22144                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22145         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22146         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22147         final NEDVelocity nedVelocity = new NEDVelocity();
22148         final ECEFPosition ecefPosition = new ECEFPosition();
22149         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22150         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22151                 ecefPosition, ecefVelocity);
22152         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22153                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22154         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22155 
22156         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
22157                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
22158                         bx, by, bz, sx, sy, sz);
22159 
22160         // check default values
22161         assertEquals(calibrator.getBiasX(), biasX, 0.0);
22162         assertEquals(calibrator.getBiasY(), biasY, 0.0);
22163         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
22164         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
22165         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22166         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22167         final Acceleration bx2 = new Acceleration(0.0,
22168                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22169         calibrator.getBiasXAsAcceleration(bx2);
22170         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22171         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22172         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
22173         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22174         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22175         final Acceleration by2 = new Acceleration(0.0,
22176                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22177         calibrator.getBiasYAsAcceleration(by2);
22178         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22179         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22180         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
22181         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22182         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22183         final Acceleration bz2 = new Acceleration(0.0,
22184                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22185         calibrator.getBiasZAsAcceleration(bz2);
22186         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22187         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22188         assertEquals(calibrator.getInitialSx(), sx, 0.0);
22189         assertEquals(calibrator.getInitialSy(), sy, 0.0);
22190         assertEquals(calibrator.getInitialSz(), sz, 0.0);
22191         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22192         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22193         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22194         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22195         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22196         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22197         final double[] bias1 = calibrator.getBias();
22198         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22199         final double[] bias2 = new double[3];
22200         calibrator.getBias(bias2);
22201         assertArrayEquals(bias1, bias2, 0.0);
22202         final Matrix b1 = calibrator.getBiasAsMatrix();
22203         assertEquals(b1, ba);
22204         final Matrix b2 = new Matrix(3, 1);
22205         calibrator.getBiasAsMatrix(b2);
22206         assertEquals(b1, b2);
22207         final Matrix ma1 = calibrator.getInitialMa();
22208         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22209         final Matrix ma2 = new Matrix(3, 3);
22210         calibrator.getInitialMa(ma2);
22211         assertEquals(ma1, ma2);
22212         assertNull(calibrator.getMeasurements());
22213         assertFalse(calibrator.isCommonAxisUsed());
22214         assertNull(calibrator.getListener());
22215         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22216         assertFalse(calibrator.isReady());
22217         assertFalse(calibrator.isRunning());
22218         assertNull(calibrator.getEstimatedMa());
22219         assertNull(calibrator.getEstimatedSx());
22220         assertNull(calibrator.getEstimatedSy());
22221         assertNull(calibrator.getEstimatedSz());
22222         assertNull(calibrator.getEstimatedMxy());
22223         assertNull(calibrator.getEstimatedMxz());
22224         assertNull(calibrator.getEstimatedMyx());
22225         assertNull(calibrator.getEstimatedMyz());
22226         assertNull(calibrator.getEstimatedMzx());
22227         assertNull(calibrator.getEstimatedMzy());
22228         assertNull(calibrator.getEstimatedCovariance());
22229         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22230         assertNotNull(calibrator.getGroundTruthGravityNorm());
22231         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22232         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22233         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22234                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22235         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22236         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22237         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22238 
22239         // Force IllegalArgumentException
22240         final Acceleration invalidGravityNorm = new Acceleration(
22241                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22242 
22243         calibrator = null;
22244         try {
22245             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
22246                     invalidGravityNorm,
22247                     bx, by, bz, sx, sy, sz);
22248             fail("IllegalArgumentException expected but not thrown");
22249         } catch (final IllegalArgumentException ignore) {
22250         }
22251         assertNull(calibrator);
22252     }
22253 
22254     @Test
22255     public void testConstructor189() throws WrongSizeException {
22256         final Matrix ba = generateBa();
22257         final double biasX = ba.getElementAtIndex(0);
22258         final double biasY = ba.getElementAtIndex(1);
22259         final double biasZ = ba.getElementAtIndex(2);
22260 
22261         final Acceleration bx = new Acceleration(biasX,
22262                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22263         final Acceleration by = new Acceleration(biasY,
22264                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22265         final Acceleration bz = new Acceleration(biasZ,
22266                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22267 
22268         final Matrix ma = generateMaCommonAxis();
22269         final double sx = ma.getElementAt(0, 0);
22270         final double sy = ma.getElementAt(1, 1);
22271         final double sz = ma.getElementAt(2, 2);
22272 
22273         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22274         final double latitude = Math.toRadians(
22275                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22276         final double longitude = Math.toRadians(
22277                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22278         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22279         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22280         final NEDVelocity nedVelocity = new NEDVelocity();
22281         final ECEFPosition ecefPosition = new ECEFPosition();
22282         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22283         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22284                 ecefPosition, ecefVelocity);
22285         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22286                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22287         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22288 
22289         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
22290                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
22291                         bx, by, bz, sx, sy, sz, this);
22292 
22293         // check default values
22294         assertEquals(calibrator.getBiasX(), biasX, 0.0);
22295         assertEquals(calibrator.getBiasY(), biasY, 0.0);
22296         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
22297         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
22298         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22299         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22300         final Acceleration bx2 = new Acceleration(0.0,
22301                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22302         calibrator.getBiasXAsAcceleration(bx2);
22303         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22304         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22305         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
22306         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22307         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22308         final Acceleration by2 = new Acceleration(0.0,
22309                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22310         calibrator.getBiasYAsAcceleration(by2);
22311         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22312         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22313         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
22314         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22315         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22316         final Acceleration bz2 = new Acceleration(0.0,
22317                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22318         calibrator.getBiasZAsAcceleration(bz2);
22319         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22320         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22321         assertEquals(calibrator.getInitialSx(), sx, 0.0);
22322         assertEquals(calibrator.getInitialSy(), sy, 0.0);
22323         assertEquals(calibrator.getInitialSz(), sz, 0.0);
22324         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22325         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22326         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22327         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22328         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22329         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22330         final double[] bias1 = calibrator.getBias();
22331         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22332         final double[] bias2 = new double[3];
22333         calibrator.getBias(bias2);
22334         assertArrayEquals(bias1, bias2, 0.0);
22335         final Matrix b1 = calibrator.getBiasAsMatrix();
22336         assertEquals(b1, ba);
22337         final Matrix b2 = new Matrix(3, 1);
22338         calibrator.getBiasAsMatrix(b2);
22339         assertEquals(b1, b2);
22340         final Matrix ma1 = calibrator.getInitialMa();
22341         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22342         final Matrix ma2 = new Matrix(3, 3);
22343         calibrator.getInitialMa(ma2);
22344         assertEquals(ma1, ma2);
22345         assertNull(calibrator.getMeasurements());
22346         assertFalse(calibrator.isCommonAxisUsed());
22347         assertSame(calibrator.getListener(), this);
22348         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22349         assertFalse(calibrator.isReady());
22350         assertFalse(calibrator.isRunning());
22351         assertNull(calibrator.getEstimatedMa());
22352         assertNull(calibrator.getEstimatedSx());
22353         assertNull(calibrator.getEstimatedSy());
22354         assertNull(calibrator.getEstimatedSz());
22355         assertNull(calibrator.getEstimatedMxy());
22356         assertNull(calibrator.getEstimatedMxz());
22357         assertNull(calibrator.getEstimatedMyx());
22358         assertNull(calibrator.getEstimatedMyz());
22359         assertNull(calibrator.getEstimatedMzx());
22360         assertNull(calibrator.getEstimatedMzy());
22361         assertNull(calibrator.getEstimatedCovariance());
22362         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22363         assertNotNull(calibrator.getGroundTruthGravityNorm());
22364         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22365         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22366         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22367                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22368         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22369         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22370         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22371 
22372         // Force IllegalArgumentException
22373         final Acceleration invalidGravityNorm = new Acceleration(
22374                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22375 
22376         calibrator = null;
22377         try {
22378             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
22379                     invalidGravityNorm, bx, by, bz, sx, sy, sz,
22380                     this);
22381             fail("IllegalArgumentException expected but not thrown");
22382         } catch (final IllegalArgumentException ignore) {
22383         }
22384         assertNull(calibrator);
22385     }
22386 
22387     @Test
22388     public void testConstructor190() throws WrongSizeException {
22389         final Collection<StandardDeviationBodyKinematics> measurements =
22390                 Collections.emptyList();
22391 
22392         final Matrix ba = generateBa();
22393         final double biasX = ba.getElementAtIndex(0);
22394         final double biasY = ba.getElementAtIndex(1);
22395         final double biasZ = ba.getElementAtIndex(2);
22396 
22397         final Acceleration bx = new Acceleration(biasX,
22398                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22399         final Acceleration by = new Acceleration(biasY,
22400                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22401         final Acceleration bz = new Acceleration(biasZ,
22402                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22403 
22404         final Matrix ma = generateMaCommonAxis();
22405         final double sx = ma.getElementAt(0, 0);
22406         final double sy = ma.getElementAt(1, 1);
22407         final double sz = ma.getElementAt(2, 2);
22408 
22409         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22410         final double latitude = Math.toRadians(
22411                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22412         final double longitude = Math.toRadians(
22413                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22414         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22415         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22416         final NEDVelocity nedVelocity = new NEDVelocity();
22417         final ECEFPosition ecefPosition = new ECEFPosition();
22418         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22419         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22420                 ecefPosition, ecefVelocity);
22421         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22422                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22423         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22424 
22425         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
22426                 new KnownBiasAndGravityNormAccelerometerCalibrator(
22427                         gravityNorm, measurements,
22428                         bx, by, bz, sx, sy, sz);
22429 
22430         // check default values
22431         assertEquals(calibrator.getBiasX(), biasX, 0.0);
22432         assertEquals(calibrator.getBiasY(), biasY, 0.0);
22433         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
22434         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
22435         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22436         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22437         final Acceleration bx2 = new Acceleration(0.0,
22438                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22439         calibrator.getBiasXAsAcceleration(bx2);
22440         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22441         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22442         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
22443         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22444         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22445         final Acceleration by2 = new Acceleration(0.0,
22446                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22447         calibrator.getBiasYAsAcceleration(by2);
22448         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22449         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22450         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
22451         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22452         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22453         final Acceleration bz2 = new Acceleration(0.0,
22454                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22455         calibrator.getBiasZAsAcceleration(bz2);
22456         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22457         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22458         assertEquals(calibrator.getInitialSx(), sx, 0.0);
22459         assertEquals(calibrator.getInitialSy(), sy, 0.0);
22460         assertEquals(calibrator.getInitialSz(), sz, 0.0);
22461         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22462         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22463         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22464         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22465         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22466         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22467         final double[] bias1 = calibrator.getBias();
22468         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22469         final double[] bias2 = new double[3];
22470         calibrator.getBias(bias2);
22471         assertArrayEquals(bias1, bias2, 0.0);
22472         final Matrix b1 = calibrator.getBiasAsMatrix();
22473         assertEquals(b1, ba);
22474         final Matrix b2 = new Matrix(3, 1);
22475         calibrator.getBiasAsMatrix(b2);
22476         assertEquals(b1, b2);
22477         final Matrix ma1 = calibrator.getInitialMa();
22478         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22479         final Matrix ma2 = new Matrix(3, 3);
22480         calibrator.getInitialMa(ma2);
22481         assertEquals(ma1, ma2);
22482         assertSame(calibrator.getMeasurements(), measurements);
22483         assertFalse(calibrator.isCommonAxisUsed());
22484         assertNull(calibrator.getListener());
22485         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22486         assertFalse(calibrator.isReady());
22487         assertFalse(calibrator.isRunning());
22488         assertNull(calibrator.getEstimatedMa());
22489         assertNull(calibrator.getEstimatedSx());
22490         assertNull(calibrator.getEstimatedSy());
22491         assertNull(calibrator.getEstimatedSz());
22492         assertNull(calibrator.getEstimatedMxy());
22493         assertNull(calibrator.getEstimatedMxz());
22494         assertNull(calibrator.getEstimatedMyx());
22495         assertNull(calibrator.getEstimatedMyz());
22496         assertNull(calibrator.getEstimatedMzx());
22497         assertNull(calibrator.getEstimatedMzy());
22498         assertNull(calibrator.getEstimatedCovariance());
22499         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22500         assertNotNull(calibrator.getGroundTruthGravityNorm());
22501         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22502         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22503         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22504                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22505         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22506         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22507         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22508 
22509         // Force IllegalArgumentException
22510         final Acceleration invalidGravityNorm = new Acceleration(
22511                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22512 
22513         calibrator = null;
22514         try {
22515             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
22516                     invalidGravityNorm, measurements,
22517                     bx, by, bz, sx, sy, sz);
22518             fail("IllegalArgumentException expected but not thrown");
22519         } catch (final IllegalArgumentException ignore) {
22520         }
22521         assertNull(calibrator);
22522     }
22523 
22524     @Test
22525     public void testConstructor191() throws WrongSizeException {
22526         final Collection<StandardDeviationBodyKinematics> measurements =
22527                 Collections.emptyList();
22528 
22529         final Matrix ba = generateBa();
22530         final double biasX = ba.getElementAtIndex(0);
22531         final double biasY = ba.getElementAtIndex(1);
22532         final double biasZ = ba.getElementAtIndex(2);
22533 
22534         final Acceleration bx = new Acceleration(biasX,
22535                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22536         final Acceleration by = new Acceleration(biasY,
22537                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22538         final Acceleration bz = new Acceleration(biasZ,
22539                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22540 
22541         final Matrix ma = generateMaCommonAxis();
22542         final double sx = ma.getElementAt(0, 0);
22543         final double sy = ma.getElementAt(1, 1);
22544         final double sz = ma.getElementAt(2, 2);
22545 
22546         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22547         final double latitude = Math.toRadians(
22548                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22549         final double longitude = Math.toRadians(
22550                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22551         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22552         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22553         final NEDVelocity nedVelocity = new NEDVelocity();
22554         final ECEFPosition ecefPosition = new ECEFPosition();
22555         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22556         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22557                 ecefPosition, ecefVelocity);
22558         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22559                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22560         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22561 
22562         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
22563                 new KnownBiasAndGravityNormAccelerometerCalibrator(
22564                         gravityNorm, measurements,
22565                         bx, by, bz, sx, sy, sz, this);
22566 
22567         // check default values
22568         assertEquals(calibrator.getBiasX(), biasX, 0.0);
22569         assertEquals(calibrator.getBiasY(), biasY, 0.0);
22570         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
22571         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
22572         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22573         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22574         final Acceleration bx2 = new Acceleration(0.0,
22575                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22576         calibrator.getBiasXAsAcceleration(bx2);
22577         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22578         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22579         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
22580         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22581         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22582         final Acceleration by2 = new Acceleration(0.0,
22583                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22584         calibrator.getBiasYAsAcceleration(by2);
22585         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22586         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22587         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
22588         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22589         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22590         final Acceleration bz2 = new Acceleration(0.0,
22591                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22592         calibrator.getBiasZAsAcceleration(bz2);
22593         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22594         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22595         assertEquals(calibrator.getInitialSx(), sx, 0.0);
22596         assertEquals(calibrator.getInitialSy(), sy, 0.0);
22597         assertEquals(calibrator.getInitialSz(), sz, 0.0);
22598         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22599         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22600         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22601         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22602         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22603         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22604         final double[] bias1 = calibrator.getBias();
22605         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22606         final double[] bias2 = new double[3];
22607         calibrator.getBias(bias2);
22608         assertArrayEquals(bias1, bias2, 0.0);
22609         final Matrix b1 = calibrator.getBiasAsMatrix();
22610         assertEquals(b1, ba);
22611         final Matrix b2 = new Matrix(3, 1);
22612         calibrator.getBiasAsMatrix(b2);
22613         assertEquals(b1, b2);
22614         final Matrix ma1 = calibrator.getInitialMa();
22615         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22616         final Matrix ma2 = new Matrix(3, 3);
22617         calibrator.getInitialMa(ma2);
22618         assertEquals(ma1, ma2);
22619         assertSame(calibrator.getMeasurements(), measurements);
22620         assertFalse(calibrator.isCommonAxisUsed());
22621         assertSame(calibrator.getListener(), this);
22622         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22623         assertFalse(calibrator.isReady());
22624         assertFalse(calibrator.isRunning());
22625         assertNull(calibrator.getEstimatedMa());
22626         assertNull(calibrator.getEstimatedSx());
22627         assertNull(calibrator.getEstimatedSy());
22628         assertNull(calibrator.getEstimatedSz());
22629         assertNull(calibrator.getEstimatedMxy());
22630         assertNull(calibrator.getEstimatedMxz());
22631         assertNull(calibrator.getEstimatedMyx());
22632         assertNull(calibrator.getEstimatedMyz());
22633         assertNull(calibrator.getEstimatedMzx());
22634         assertNull(calibrator.getEstimatedMzy());
22635         assertNull(calibrator.getEstimatedCovariance());
22636         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22637         assertNotNull(calibrator.getGroundTruthGravityNorm());
22638         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22639         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22640         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22641                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22642         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22643         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22644         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22645 
22646         // Force IllegalArgumentException
22647         final Acceleration invalidGravityNorm = new Acceleration(
22648                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22649 
22650         calibrator = null;
22651         try {
22652             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
22653                     invalidGravityNorm, measurements,
22654                     bx, by, bz, sx, sy, sz, this);
22655             fail("IllegalArgumentException expected but not thrown");
22656         } catch (final IllegalArgumentException ignore) {
22657         }
22658         assertNull(calibrator);
22659     }
22660 
22661     @Test
22662     public void testConstructor192() throws WrongSizeException {
22663         final Matrix ba = generateBa();
22664         final double biasX = ba.getElementAtIndex(0);
22665         final double biasY = ba.getElementAtIndex(1);
22666         final double biasZ = ba.getElementAtIndex(2);
22667 
22668         final Acceleration bx = new Acceleration(biasX,
22669                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22670         final Acceleration by = new Acceleration(biasY,
22671                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22672         final Acceleration bz = new Acceleration(biasZ,
22673                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22674 
22675         final Matrix ma = generateMaCommonAxis();
22676         final double sx = ma.getElementAt(0, 0);
22677         final double sy = ma.getElementAt(1, 1);
22678         final double sz = ma.getElementAt(2, 2);
22679 
22680         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22681         final double latitude = Math.toRadians(
22682                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22683         final double longitude = Math.toRadians(
22684                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22685         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22686         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22687         final NEDVelocity nedVelocity = new NEDVelocity();
22688         final ECEFPosition ecefPosition = new ECEFPosition();
22689         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22690         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22691                 ecefPosition, ecefVelocity);
22692         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22693                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22694         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22695 
22696         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
22697                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
22698                         true, bx, by, bz, sx, sy, sz);
22699 
22700         // check default values
22701         assertEquals(calibrator.getBiasX(), biasX, 0.0);
22702         assertEquals(calibrator.getBiasY(), biasY, 0.0);
22703         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
22704         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
22705         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22706         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22707         final Acceleration bx2 = new Acceleration(0.0,
22708                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22709         calibrator.getBiasXAsAcceleration(bx2);
22710         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22711         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22712         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
22713         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22714         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22715         final Acceleration by2 = new Acceleration(0.0,
22716                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22717         calibrator.getBiasYAsAcceleration(by2);
22718         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22719         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22720         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
22721         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22722         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22723         final Acceleration bz2 = new Acceleration(0.0,
22724                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22725         calibrator.getBiasZAsAcceleration(bz2);
22726         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22727         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22728         assertEquals(calibrator.getInitialSx(), sx, 0.0);
22729         assertEquals(calibrator.getInitialSy(), sy, 0.0);
22730         assertEquals(calibrator.getInitialSz(), sz, 0.0);
22731         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22732         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22733         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22734         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22735         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22736         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22737         final double[] bias1 = calibrator.getBias();
22738         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22739         final double[] bias2 = new double[3];
22740         calibrator.getBias(bias2);
22741         assertArrayEquals(bias1, bias2, 0.0);
22742         final Matrix b1 = calibrator.getBiasAsMatrix();
22743         assertEquals(b1, ba);
22744         final Matrix b2 = new Matrix(3, 1);
22745         calibrator.getBiasAsMatrix(b2);
22746         assertEquals(b1, b2);
22747         final Matrix ma1 = calibrator.getInitialMa();
22748         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22749         final Matrix ma2 = new Matrix(3, 3);
22750         calibrator.getInitialMa(ma2);
22751         assertEquals(ma1, ma2);
22752         assertNull(calibrator.getMeasurements());
22753         assertTrue(calibrator.isCommonAxisUsed());
22754         assertNull(calibrator.getListener());
22755         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
22756         assertFalse(calibrator.isReady());
22757         assertFalse(calibrator.isRunning());
22758         assertNull(calibrator.getEstimatedMa());
22759         assertNull(calibrator.getEstimatedSx());
22760         assertNull(calibrator.getEstimatedSy());
22761         assertNull(calibrator.getEstimatedSz());
22762         assertNull(calibrator.getEstimatedMxy());
22763         assertNull(calibrator.getEstimatedMxz());
22764         assertNull(calibrator.getEstimatedMyx());
22765         assertNull(calibrator.getEstimatedMyz());
22766         assertNull(calibrator.getEstimatedMzx());
22767         assertNull(calibrator.getEstimatedMzy());
22768         assertNull(calibrator.getEstimatedCovariance());
22769         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22770         assertNotNull(calibrator.getGroundTruthGravityNorm());
22771         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22772         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22773         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22774                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22775         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22776         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22777         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22778 
22779         // Force IllegalArgumentException
22780         final Acceleration invalidGravityNorm = new Acceleration(
22781                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22782 
22783         calibrator = null;
22784         try {
22785             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
22786                     invalidGravityNorm, true,
22787                     bx, by, bz, sx, sy, sz);
22788             fail("IllegalArgumentException expected but not thrown");
22789         } catch (final IllegalArgumentException ignore) {
22790         }
22791         assertNull(calibrator);
22792     }
22793 
22794     @Test
22795     public void testConstructor193() throws WrongSizeException {
22796         final Matrix ba = generateBa();
22797         final double biasX = ba.getElementAtIndex(0);
22798         final double biasY = ba.getElementAtIndex(1);
22799         final double biasZ = ba.getElementAtIndex(2);
22800 
22801         final Acceleration bx = new Acceleration(biasX,
22802                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22803         final Acceleration by = new Acceleration(biasY,
22804                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22805         final Acceleration bz = new Acceleration(biasZ,
22806                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22807 
22808         final Matrix ma = generateMaCommonAxis();
22809         final double sx = ma.getElementAt(0, 0);
22810         final double sy = ma.getElementAt(1, 1);
22811         final double sz = ma.getElementAt(2, 2);
22812 
22813         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22814         final double latitude = Math.toRadians(
22815                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22816         final double longitude = Math.toRadians(
22817                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22818         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22819         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22820         final NEDVelocity nedVelocity = new NEDVelocity();
22821         final ECEFPosition ecefPosition = new ECEFPosition();
22822         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22823         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22824                 ecefPosition, ecefVelocity);
22825         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22826                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22827         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22828 
22829         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
22830                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
22831                         true, bx, by, bz, sx, sy, sz, this);
22832 
22833         // check default values
22834         assertEquals(calibrator.getBiasX(), biasX, 0.0);
22835         assertEquals(calibrator.getBiasY(), biasY, 0.0);
22836         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
22837         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
22838         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22839         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22840         final Acceleration bx2 = new Acceleration(0.0,
22841                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22842         calibrator.getBiasXAsAcceleration(bx2);
22843         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22844         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22845         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
22846         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22847         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22848         final Acceleration by2 = new Acceleration(0.0,
22849                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22850         calibrator.getBiasYAsAcceleration(by2);
22851         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22852         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22853         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
22854         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22855         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22856         final Acceleration bz2 = new Acceleration(0.0,
22857                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22858         calibrator.getBiasZAsAcceleration(bz2);
22859         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22860         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22861         assertEquals(calibrator.getInitialSx(), sx, 0.0);
22862         assertEquals(calibrator.getInitialSy(), sy, 0.0);
22863         assertEquals(calibrator.getInitialSz(), sz, 0.0);
22864         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22865         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22866         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22867         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22868         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22869         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22870         final double[] bias1 = calibrator.getBias();
22871         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22872         final double[] bias2 = new double[3];
22873         calibrator.getBias(bias2);
22874         assertArrayEquals(bias1, bias2, 0.0);
22875         final Matrix b1 = calibrator.getBiasAsMatrix();
22876         assertEquals(b1, ba);
22877         final Matrix b2 = new Matrix(3, 1);
22878         calibrator.getBiasAsMatrix(b2);
22879         assertEquals(b1, b2);
22880         final Matrix ma1 = calibrator.getInitialMa();
22881         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22882         final Matrix ma2 = new Matrix(3, 3);
22883         calibrator.getInitialMa(ma2);
22884         assertEquals(ma1, ma2);
22885         assertNull(calibrator.getMeasurements());
22886         assertTrue(calibrator.isCommonAxisUsed());
22887         assertSame(calibrator.getListener(), this);
22888         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
22889         assertFalse(calibrator.isReady());
22890         assertFalse(calibrator.isRunning());
22891         assertNull(calibrator.getEstimatedMa());
22892         assertNull(calibrator.getEstimatedSx());
22893         assertNull(calibrator.getEstimatedSy());
22894         assertNull(calibrator.getEstimatedSz());
22895         assertNull(calibrator.getEstimatedMxy());
22896         assertNull(calibrator.getEstimatedMxz());
22897         assertNull(calibrator.getEstimatedMyx());
22898         assertNull(calibrator.getEstimatedMyz());
22899         assertNull(calibrator.getEstimatedMzx());
22900         assertNull(calibrator.getEstimatedMzy());
22901         assertNull(calibrator.getEstimatedCovariance());
22902         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22903         assertNotNull(calibrator.getGroundTruthGravityNorm());
22904         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22905         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22906         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22907                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22908         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22909         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22910         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22911 
22912         // Force IllegalArgumentException
22913         final Acceleration invalidGravityNorm = new Acceleration(
22914                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22915 
22916         calibrator = null;
22917         try {
22918             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
22919                     invalidGravityNorm, true,
22920                     bx, by, bz, sx, sy, sz, this);
22921             fail("IllegalArgumentException expected but not thrown");
22922         } catch (final IllegalArgumentException ignore) {
22923         }
22924         assertNull(calibrator);
22925     }
22926 
22927     @Test
22928     public void testConstructor194() throws WrongSizeException {
22929         final Collection<StandardDeviationBodyKinematics> measurements =
22930                 Collections.emptyList();
22931 
22932         final Matrix ba = generateBa();
22933         final double biasX = ba.getElementAtIndex(0);
22934         final double biasY = ba.getElementAtIndex(1);
22935         final double biasZ = ba.getElementAtIndex(2);
22936 
22937         final Acceleration bx = new Acceleration(biasX,
22938                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22939         final Acceleration by = new Acceleration(biasY,
22940                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22941         final Acceleration bz = new Acceleration(biasZ,
22942                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22943 
22944         final Matrix ma = generateMaCommonAxis();
22945         final double sx = ma.getElementAt(0, 0);
22946         final double sy = ma.getElementAt(1, 1);
22947         final double sz = ma.getElementAt(2, 2);
22948 
22949         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22950         final double latitude = Math.toRadians(
22951                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22952         final double longitude = Math.toRadians(
22953                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22954         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22955         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22956         final NEDVelocity nedVelocity = new NEDVelocity();
22957         final ECEFPosition ecefPosition = new ECEFPosition();
22958         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22959         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22960                 ecefPosition, ecefVelocity);
22961         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22962                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22963         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22964 
22965         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
22966                 new KnownBiasAndGravityNormAccelerometerCalibrator(
22967                         gravityNorm, measurements,
22968                         true, bx, by, bz, sx, sy, sz);
22969 
22970         // check default values
22971         assertEquals(calibrator.getBiasX(), biasX, 0.0);
22972         assertEquals(calibrator.getBiasY(), biasY, 0.0);
22973         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
22974         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
22975         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22976         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22977         final Acceleration bx2 = new Acceleration(0.0,
22978                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22979         calibrator.getBiasXAsAcceleration(bx2);
22980         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22981         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22982         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
22983         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22984         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22985         final Acceleration by2 = new Acceleration(0.0,
22986                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22987         calibrator.getBiasYAsAcceleration(by2);
22988         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22989         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22990         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
22991         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22992         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22993         final Acceleration bz2 = new Acceleration(0.0,
22994                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22995         calibrator.getBiasZAsAcceleration(bz2);
22996         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22997         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22998         assertEquals(calibrator.getInitialSx(), sx, 0.0);
22999         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23000         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23001         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23002         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23003         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23004         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23005         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23006         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23007         final double[] bias1 = calibrator.getBias();
23008         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23009         final double[] bias2 = new double[3];
23010         calibrator.getBias(bias2);
23011         assertArrayEquals(bias1, bias2, 0.0);
23012         final Matrix b1 = calibrator.getBiasAsMatrix();
23013         assertEquals(b1, ba);
23014         final Matrix b2 = new Matrix(3, 1);
23015         calibrator.getBiasAsMatrix(b2);
23016         assertEquals(b1, b2);
23017         final Matrix ma1 = calibrator.getInitialMa();
23018         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23019         final Matrix ma2 = new Matrix(3, 3);
23020         calibrator.getInitialMa(ma2);
23021         assertEquals(ma1, ma2);
23022         assertSame(calibrator.getMeasurements(), measurements);
23023         assertTrue(calibrator.isCommonAxisUsed());
23024         assertNull(calibrator.getListener());
23025         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
23026         assertFalse(calibrator.isReady());
23027         assertFalse(calibrator.isRunning());
23028         assertNull(calibrator.getEstimatedMa());
23029         assertNull(calibrator.getEstimatedSx());
23030         assertNull(calibrator.getEstimatedSy());
23031         assertNull(calibrator.getEstimatedSz());
23032         assertNull(calibrator.getEstimatedMxy());
23033         assertNull(calibrator.getEstimatedMxz());
23034         assertNull(calibrator.getEstimatedMyx());
23035         assertNull(calibrator.getEstimatedMyz());
23036         assertNull(calibrator.getEstimatedMzx());
23037         assertNull(calibrator.getEstimatedMzy());
23038         assertNull(calibrator.getEstimatedCovariance());
23039         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23040         assertNotNull(calibrator.getGroundTruthGravityNorm());
23041         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23042         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23043         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23044                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23045         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23046         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23047         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23048 
23049         // Force IllegalArgumentException
23050         final Acceleration invalidGravityNorm = new Acceleration(
23051                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23052 
23053         calibrator = null;
23054         try {
23055             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
23056                     invalidGravityNorm, measurements,
23057                     true, bx, by, bz, sx, sy, sz);
23058             fail("IllegalArgumentException expected but not thrown");
23059         } catch (final IllegalArgumentException ignore) {
23060         }
23061         assertNull(calibrator);
23062     }
23063 
23064     @Test
23065     public void testConstructor195() throws WrongSizeException {
23066         final Collection<StandardDeviationBodyKinematics> measurements =
23067                 Collections.emptyList();
23068 
23069         final Matrix ba = generateBa();
23070         final double biasX = ba.getElementAtIndex(0);
23071         final double biasY = ba.getElementAtIndex(1);
23072         final double biasZ = ba.getElementAtIndex(2);
23073 
23074         final Acceleration bx = new Acceleration(biasX,
23075                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23076         final Acceleration by = new Acceleration(biasY,
23077                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23078         final Acceleration bz = new Acceleration(biasZ,
23079                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23080 
23081         final Matrix ma = generateMaCommonAxis();
23082         final double sx = ma.getElementAt(0, 0);
23083         final double sy = ma.getElementAt(1, 1);
23084         final double sz = ma.getElementAt(2, 2);
23085 
23086         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23087         final double latitude = Math.toRadians(
23088                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23089         final double longitude = Math.toRadians(
23090                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23091         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23092         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23093         final NEDVelocity nedVelocity = new NEDVelocity();
23094         final ECEFPosition ecefPosition = new ECEFPosition();
23095         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23096         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23097                 ecefPosition, ecefVelocity);
23098         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23099                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23100         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23101 
23102         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
23103                 new KnownBiasAndGravityNormAccelerometerCalibrator(
23104                         gravityNorm, measurements,
23105                         true, bx, by, bz, sx, sy, sz, this);
23106 
23107         // check default values
23108         assertEquals(calibrator.getBiasX(), biasX, 0.0);
23109         assertEquals(calibrator.getBiasY(), biasY, 0.0);
23110         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
23111         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
23112         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23113         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23114         final Acceleration bx2 = new Acceleration(0.0,
23115                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23116         calibrator.getBiasXAsAcceleration(bx2);
23117         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23118         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23119         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
23120         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23121         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23122         final Acceleration by2 = new Acceleration(0.0,
23123                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23124         calibrator.getBiasYAsAcceleration(by2);
23125         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23126         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23127         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
23128         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23129         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23130         final Acceleration bz2 = new Acceleration(0.0,
23131                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23132         calibrator.getBiasZAsAcceleration(bz2);
23133         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23134         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23135         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23136         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23137         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23138         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23139         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23140         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23141         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23142         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23143         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23144         final double[] bias1 = calibrator.getBias();
23145         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23146         final double[] bias2 = new double[3];
23147         calibrator.getBias(bias2);
23148         assertArrayEquals(bias1, bias2, 0.0);
23149         final Matrix b1 = calibrator.getBiasAsMatrix();
23150         assertEquals(b1, ba);
23151         final Matrix b2 = new Matrix(3, 1);
23152         calibrator.getBiasAsMatrix(b2);
23153         assertEquals(b1, b2);
23154         final Matrix ma1 = calibrator.getInitialMa();
23155         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23156         final Matrix ma2 = new Matrix(3, 3);
23157         calibrator.getInitialMa(ma2);
23158         assertEquals(ma1, ma2);
23159         assertSame(calibrator.getMeasurements(), measurements);
23160         assertTrue(calibrator.isCommonAxisUsed());
23161         assertSame(calibrator.getListener(), this);
23162         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
23163         assertFalse(calibrator.isReady());
23164         assertFalse(calibrator.isRunning());
23165         assertNull(calibrator.getEstimatedMa());
23166         assertNull(calibrator.getEstimatedSx());
23167         assertNull(calibrator.getEstimatedSy());
23168         assertNull(calibrator.getEstimatedSz());
23169         assertNull(calibrator.getEstimatedMxy());
23170         assertNull(calibrator.getEstimatedMxz());
23171         assertNull(calibrator.getEstimatedMyx());
23172         assertNull(calibrator.getEstimatedMyz());
23173         assertNull(calibrator.getEstimatedMzx());
23174         assertNull(calibrator.getEstimatedMzy());
23175         assertNull(calibrator.getEstimatedCovariance());
23176         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23177         assertNotNull(calibrator.getGroundTruthGravityNorm());
23178         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23179         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23180         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23181                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23182         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23183         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23184         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23185 
23186         // Force IllegalArgumentException
23187         final Acceleration invalidGravityNorm = new Acceleration(
23188                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23189 
23190         calibrator = null;
23191         try {
23192             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
23193                     invalidGravityNorm, measurements,
23194                     true, bx, by, bz, sx, sy, sz, this);
23195             fail("IllegalArgumentException expected but not thrown");
23196         } catch (final IllegalArgumentException ignore) {
23197         }
23198         assertNull(calibrator);
23199     }
23200 
23201     @Test
23202     public void testConstructor196() throws WrongSizeException {
23203         final Matrix ba = generateBa();
23204         final double biasX = ba.getElementAtIndex(0);
23205         final double biasY = ba.getElementAtIndex(1);
23206         final double biasZ = ba.getElementAtIndex(2);
23207 
23208         final Matrix ma = generateMaCommonAxis();
23209         final double sx = ma.getElementAt(0, 0);
23210         final double sy = ma.getElementAt(1, 1);
23211         final double sz = ma.getElementAt(2, 2);
23212         final double mxy = ma.getElementAt(0, 1);
23213         final double mxz = ma.getElementAt(0, 2);
23214         final double myx = ma.getElementAt(1, 0);
23215         final double myz = ma.getElementAt(1, 2);
23216         final double mzx = ma.getElementAt(2, 0);
23217         final double mzy = ma.getElementAt(2, 1);
23218 
23219         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23220         final double latitude = Math.toRadians(
23221                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23222         final double longitude = Math.toRadians(
23223                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23224         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23225         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23226         final NEDVelocity nedVelocity = new NEDVelocity();
23227         final ECEFPosition ecefPosition = new ECEFPosition();
23228         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23229         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23230                 ecefPosition, ecefVelocity);
23231         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23232                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23233         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23234 
23235         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
23236                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
23237                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23238                         myx, myz, mzx, mzy);
23239 
23240         // check default values
23241         assertEquals(calibrator.getBiasX(), biasX, 0.0);
23242         assertEquals(calibrator.getBiasY(), biasY, 0.0);
23243         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
23244         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
23245         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23246         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23247         final Acceleration bx2 = new Acceleration(0.0,
23248                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23249         calibrator.getBiasXAsAcceleration(bx2);
23250         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23251         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23252         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
23253         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23254         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23255         final Acceleration by2 = new Acceleration(0.0,
23256                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23257         calibrator.getBiasYAsAcceleration(by2);
23258         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23259         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23260         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
23261         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23262         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23263         final Acceleration bz2 = new Acceleration(0.0,
23264                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23265         calibrator.getBiasZAsAcceleration(bz2);
23266         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23267         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23268         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23269         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23270         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23271         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
23272         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
23273         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
23274         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
23275         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
23276         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
23277         final double[] bias1 = calibrator.getBias();
23278         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23279         final double[] bias2 = new double[3];
23280         calibrator.getBias(bias2);
23281         assertArrayEquals(bias1, bias2, 0.0);
23282         final Matrix b1 = calibrator.getBiasAsMatrix();
23283         assertEquals(b1, ba);
23284         final Matrix b2 = new Matrix(3, 1);
23285         calibrator.getBiasAsMatrix(b2);
23286         assertEquals(b1, b2);
23287         final Matrix ma1 = new Matrix(3, 3);
23288         ma1.setSubmatrix(0, 0,
23289                 2, 2,
23290                 new double[]{sx, myx, mzx,
23291                         mxy, sy, mzy,
23292                         mxz, myz, sz});
23293         assertEquals(calibrator.getInitialMa(), ma1);
23294         final Matrix ma2 = new Matrix(3, 3);
23295         calibrator.getInitialMa(ma2);
23296         assertEquals(ma1, ma2);
23297         assertNull(calibrator.getMeasurements());
23298         assertFalse(calibrator.isCommonAxisUsed());
23299         assertNull(calibrator.getListener());
23300         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23301         assertFalse(calibrator.isReady());
23302         assertFalse(calibrator.isRunning());
23303         assertNull(calibrator.getEstimatedMa());
23304         assertNull(calibrator.getEstimatedSx());
23305         assertNull(calibrator.getEstimatedSy());
23306         assertNull(calibrator.getEstimatedSz());
23307         assertNull(calibrator.getEstimatedMxy());
23308         assertNull(calibrator.getEstimatedMxz());
23309         assertNull(calibrator.getEstimatedMyx());
23310         assertNull(calibrator.getEstimatedMyz());
23311         assertNull(calibrator.getEstimatedMzx());
23312         assertNull(calibrator.getEstimatedMzy());
23313         assertNull(calibrator.getEstimatedCovariance());
23314         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23315         assertNotNull(calibrator.getGroundTruthGravityNorm());
23316         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23317         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23318         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23319                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23320         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23321         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23322         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23323 
23324         // Force IllegalArgumentException
23325         final Acceleration invalidGravityNorm = new Acceleration(
23326                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23327 
23328         calibrator = null;
23329         try {
23330             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
23331                     invalidGravityNorm, biasX, biasY, biasZ, sx, sy, sz,
23332                     mxy, mxz, myx, myz, mzx, mzy);
23333             fail("IllegalArgumentException expected but not thrown");
23334         } catch (final IllegalArgumentException ignore) {
23335         }
23336         assertNull(calibrator);
23337     }
23338 
23339     @Test
23340     public void testConstructor197() throws WrongSizeException {
23341         final Collection<StandardDeviationBodyKinematics> measurements =
23342                 Collections.emptyList();
23343 
23344         final Matrix ba = generateBa();
23345         final double biasX = ba.getElementAtIndex(0);
23346         final double biasY = ba.getElementAtIndex(1);
23347         final double biasZ = ba.getElementAtIndex(2);
23348 
23349         final Matrix ma = generateMaCommonAxis();
23350         final double sx = ma.getElementAt(0, 0);
23351         final double sy = ma.getElementAt(1, 1);
23352         final double sz = ma.getElementAt(2, 2);
23353         final double mxy = ma.getElementAt(0, 1);
23354         final double mxz = ma.getElementAt(0, 2);
23355         final double myx = ma.getElementAt(1, 0);
23356         final double myz = ma.getElementAt(1, 2);
23357         final double mzx = ma.getElementAt(2, 0);
23358         final double mzy = ma.getElementAt(2, 1);
23359 
23360         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23361         final double latitude = Math.toRadians(
23362                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23363         final double longitude = Math.toRadians(
23364                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23365         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23366         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23367         final NEDVelocity nedVelocity = new NEDVelocity();
23368         final ECEFPosition ecefPosition = new ECEFPosition();
23369         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23370         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23371                 ecefPosition, ecefVelocity);
23372         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23373                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23374         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23375 
23376         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
23377                 new KnownBiasAndGravityNormAccelerometerCalibrator(
23378                         gravityNorm, measurements,
23379                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23380                         myx, myz, mzx, mzy);
23381 
23382         // check default values
23383         assertEquals(calibrator.getBiasX(), biasX, 0.0);
23384         assertEquals(calibrator.getBiasY(), biasY, 0.0);
23385         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
23386         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
23387         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23388         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23389         final Acceleration bx2 = new Acceleration(0.0,
23390                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23391         calibrator.getBiasXAsAcceleration(bx2);
23392         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23393         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23394         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
23395         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23396         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23397         final Acceleration by2 = new Acceleration(0.0,
23398                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23399         calibrator.getBiasYAsAcceleration(by2);
23400         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23401         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23402         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
23403         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23404         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23405         final Acceleration bz2 = new Acceleration(0.0,
23406                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23407         calibrator.getBiasZAsAcceleration(bz2);
23408         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23409         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23410         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23411         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23412         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23413         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
23414         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
23415         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
23416         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
23417         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
23418         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
23419         final double[] bias1 = calibrator.getBias();
23420         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23421         final double[] bias2 = new double[3];
23422         calibrator.getBias(bias2);
23423         assertArrayEquals(bias1, bias2, 0.0);
23424         final Matrix b1 = calibrator.getBiasAsMatrix();
23425         assertEquals(b1, ba);
23426         final Matrix b2 = new Matrix(3, 1);
23427         calibrator.getBiasAsMatrix(b2);
23428         assertEquals(b1, b2);
23429         final Matrix ma1 = new Matrix(3, 3);
23430         ma1.setSubmatrix(0, 0,
23431                 2, 2,
23432                 new double[]{sx, myx, mzx,
23433                         mxy, sy, mzy,
23434                         mxz, myz, sz});
23435         assertEquals(calibrator.getInitialMa(), ma1);
23436         final Matrix ma2 = new Matrix(3, 3);
23437         calibrator.getInitialMa(ma2);
23438         assertEquals(ma1, ma2);
23439         assertSame(calibrator.getMeasurements(), measurements);
23440         assertFalse(calibrator.isCommonAxisUsed());
23441         assertNull(calibrator.getListener());
23442         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23443         assertFalse(calibrator.isReady());
23444         assertFalse(calibrator.isRunning());
23445         assertNull(calibrator.getEstimatedMa());
23446         assertNull(calibrator.getEstimatedSx());
23447         assertNull(calibrator.getEstimatedSy());
23448         assertNull(calibrator.getEstimatedSz());
23449         assertNull(calibrator.getEstimatedMxy());
23450         assertNull(calibrator.getEstimatedMxz());
23451         assertNull(calibrator.getEstimatedMyx());
23452         assertNull(calibrator.getEstimatedMyz());
23453         assertNull(calibrator.getEstimatedMzx());
23454         assertNull(calibrator.getEstimatedMzy());
23455         assertNull(calibrator.getEstimatedCovariance());
23456         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23457         assertNotNull(calibrator.getGroundTruthGravityNorm());
23458         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23459         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23460         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23461                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23462         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23463         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23464         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23465 
23466         // Force IllegalArgumentException
23467         final Acceleration invalidGravityNorm = new Acceleration(
23468                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23469 
23470         calibrator = null;
23471         try {
23472             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
23473                     invalidGravityNorm, measurements,
23474                     biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23475                     myx, myz, mzx, mzy);
23476             fail("IllegalArgumentException expected but not thrown");
23477         } catch (final IllegalArgumentException ignore) {
23478         }
23479         assertNull(calibrator);
23480     }
23481 
23482     @Test
23483     public void testConstructor198() throws WrongSizeException {
23484         final Collection<StandardDeviationBodyKinematics> measurements =
23485                 Collections.emptyList();
23486 
23487         final Matrix ba = generateBa();
23488         final double biasX = ba.getElementAtIndex(0);
23489         final double biasY = ba.getElementAtIndex(1);
23490         final double biasZ = ba.getElementAtIndex(2);
23491 
23492         final Matrix ma = generateMaCommonAxis();
23493         final double sx = ma.getElementAt(0, 0);
23494         final double sy = ma.getElementAt(1, 1);
23495         final double sz = ma.getElementAt(2, 2);
23496         final double mxy = ma.getElementAt(0, 1);
23497         final double mxz = ma.getElementAt(0, 2);
23498         final double myx = ma.getElementAt(1, 0);
23499         final double myz = ma.getElementAt(1, 2);
23500         final double mzx = ma.getElementAt(2, 0);
23501         final double mzy = ma.getElementAt(2, 1);
23502 
23503         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23504         final double latitude = Math.toRadians(
23505                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23506         final double longitude = Math.toRadians(
23507                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23508         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23509         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23510         final NEDVelocity nedVelocity = new NEDVelocity();
23511         final ECEFPosition ecefPosition = new ECEFPosition();
23512         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23513         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23514                 ecefPosition, ecefVelocity);
23515         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23516                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23517         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23518 
23519         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
23520                 new KnownBiasAndGravityNormAccelerometerCalibrator(
23521                         gravityNorm, measurements,
23522                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23523                         myx, myz, mzx, mzy, this);
23524 
23525         // check default values
23526         assertEquals(calibrator.getBiasX(), biasX, 0.0);
23527         assertEquals(calibrator.getBiasY(), biasY, 0.0);
23528         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
23529         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
23530         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23531         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23532         final Acceleration bx2 = new Acceleration(0.0,
23533                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23534         calibrator.getBiasXAsAcceleration(bx2);
23535         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23536         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23537         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
23538         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23539         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23540         final Acceleration by2 = new Acceleration(0.0,
23541                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23542         calibrator.getBiasYAsAcceleration(by2);
23543         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23544         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23545         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
23546         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23547         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23548         final Acceleration bz2 = new Acceleration(0.0,
23549                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23550         calibrator.getBiasZAsAcceleration(bz2);
23551         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23552         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23553         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23554         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23555         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23556         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
23557         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
23558         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
23559         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
23560         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
23561         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
23562         final double[] bias1 = calibrator.getBias();
23563         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23564         final double[] bias2 = new double[3];
23565         calibrator.getBias(bias2);
23566         assertArrayEquals(bias1, bias2, 0.0);
23567         final Matrix b1 = calibrator.getBiasAsMatrix();
23568         assertEquals(b1, ba);
23569         final Matrix b2 = new Matrix(3, 1);
23570         calibrator.getBiasAsMatrix(b2);
23571         assertEquals(b1, b2);
23572         final Matrix ma1 = new Matrix(3, 3);
23573         ma1.setSubmatrix(0, 0,
23574                 2, 2,
23575                 new double[]{sx, myx, mzx,
23576                         mxy, sy, mzy,
23577                         mxz, myz, sz});
23578         assertEquals(calibrator.getInitialMa(), ma1);
23579         final Matrix ma2 = new Matrix(3, 3);
23580         calibrator.getInitialMa(ma2);
23581         assertEquals(ma1, ma2);
23582         assertSame(calibrator.getMeasurements(), measurements);
23583         assertFalse(calibrator.isCommonAxisUsed());
23584         assertSame(calibrator.getListener(), this);
23585         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23586         assertFalse(calibrator.isReady());
23587         assertFalse(calibrator.isRunning());
23588         assertNull(calibrator.getEstimatedMa());
23589         assertNull(calibrator.getEstimatedSx());
23590         assertNull(calibrator.getEstimatedSy());
23591         assertNull(calibrator.getEstimatedSz());
23592         assertNull(calibrator.getEstimatedMxy());
23593         assertNull(calibrator.getEstimatedMxz());
23594         assertNull(calibrator.getEstimatedMyx());
23595         assertNull(calibrator.getEstimatedMyz());
23596         assertNull(calibrator.getEstimatedMzx());
23597         assertNull(calibrator.getEstimatedMzy());
23598         assertNull(calibrator.getEstimatedCovariance());
23599         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23600         assertNotNull(calibrator.getGroundTruthGravityNorm());
23601         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23602         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23603         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23604                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23605         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23606         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23607         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23608 
23609         // Force IllegalArgumentException
23610         final Acceleration invalidGravityNorm = new Acceleration(
23611                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23612 
23613         calibrator = null;
23614         try {
23615             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
23616                     invalidGravityNorm, measurements,
23617                     biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23618                     myx, myz, mzx, mzy, this);
23619             fail("IllegalArgumentException expected but not thrown");
23620         } catch (final IllegalArgumentException ignore) {
23621         }
23622         assertNull(calibrator);
23623     }
23624 
23625     @Test
23626     public void testConstructor199() throws WrongSizeException {
23627         final Matrix ba = generateBa();
23628         final double biasX = ba.getElementAtIndex(0);
23629         final double biasY = ba.getElementAtIndex(1);
23630         final double biasZ = ba.getElementAtIndex(2);
23631 
23632         final Matrix ma = generateMaCommonAxis();
23633         final double sx = ma.getElementAt(0, 0);
23634         final double sy = ma.getElementAt(1, 1);
23635         final double sz = ma.getElementAt(2, 2);
23636         final double mxy = ma.getElementAt(0, 1);
23637         final double mxz = ma.getElementAt(0, 2);
23638         final double myx = ma.getElementAt(1, 0);
23639         final double myz = ma.getElementAt(1, 2);
23640         final double mzx = ma.getElementAt(2, 0);
23641         final double mzy = ma.getElementAt(2, 1);
23642 
23643         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23644         final double latitude = Math.toRadians(
23645                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23646         final double longitude = Math.toRadians(
23647                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23648         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23649         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23650         final NEDVelocity nedVelocity = new NEDVelocity();
23651         final ECEFPosition ecefPosition = new ECEFPosition();
23652         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23653         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23654                 ecefPosition, ecefVelocity);
23655         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23656                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23657         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23658 
23659         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
23660                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
23661                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23662                         myx, myz, mzx, mzy);
23663 
23664         // check default values
23665         assertEquals(calibrator.getBiasX(), biasX, 0.0);
23666         assertEquals(calibrator.getBiasY(), biasY, 0.0);
23667         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
23668         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
23669         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23670         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23671         final Acceleration bx2 = new Acceleration(0.0,
23672                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23673         calibrator.getBiasXAsAcceleration(bx2);
23674         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23675         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23676         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
23677         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23678         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23679         final Acceleration by2 = new Acceleration(0.0,
23680                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23681         calibrator.getBiasYAsAcceleration(by2);
23682         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23683         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23684         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
23685         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23686         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23687         final Acceleration bz2 = new Acceleration(0.0,
23688                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23689         calibrator.getBiasZAsAcceleration(bz2);
23690         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23691         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23692         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23693         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23694         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23695         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
23696         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
23697         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
23698         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
23699         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
23700         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
23701         final double[] bias1 = calibrator.getBias();
23702         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23703         final double[] bias2 = new double[3];
23704         calibrator.getBias(bias2);
23705         assertArrayEquals(bias1, bias2, 0.0);
23706         final Matrix b1 = calibrator.getBiasAsMatrix();
23707         assertEquals(b1, ba);
23708         final Matrix b2 = new Matrix(3, 1);
23709         calibrator.getBiasAsMatrix(b2);
23710         assertEquals(b1, b2);
23711         final Matrix ma1 = new Matrix(3, 3);
23712         ma1.setSubmatrix(0, 0,
23713                 2, 2,
23714                 new double[]{sx, myx, mzx,
23715                         mxy, sy, mzy,
23716                         mxz, myz, sz});
23717         assertEquals(calibrator.getInitialMa(), ma1);
23718         final Matrix ma2 = new Matrix(3, 3);
23719         calibrator.getInitialMa(ma2);
23720         assertEquals(ma1, ma2);
23721         assertNull(calibrator.getMeasurements());
23722         assertTrue(calibrator.isCommonAxisUsed());
23723         assertNull(calibrator.getListener());
23724         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
23725         assertFalse(calibrator.isReady());
23726         assertFalse(calibrator.isRunning());
23727         assertNull(calibrator.getEstimatedMa());
23728         assertNull(calibrator.getEstimatedSx());
23729         assertNull(calibrator.getEstimatedSy());
23730         assertNull(calibrator.getEstimatedSz());
23731         assertNull(calibrator.getEstimatedMxy());
23732         assertNull(calibrator.getEstimatedMxz());
23733         assertNull(calibrator.getEstimatedMyx());
23734         assertNull(calibrator.getEstimatedMyz());
23735         assertNull(calibrator.getEstimatedMzx());
23736         assertNull(calibrator.getEstimatedMzy());
23737         assertNull(calibrator.getEstimatedCovariance());
23738         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23739         assertNotNull(calibrator.getGroundTruthGravityNorm());
23740         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23741         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23742         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23743                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23744         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23745         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23746         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23747 
23748         // Force IllegalArgumentException
23749         final Acceleration invalidGravityNorm = new Acceleration(
23750                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23751 
23752         calibrator = null;
23753         try {
23754             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
23755                     invalidGravityNorm, true, biasX, biasY, biasZ,
23756                     sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
23757             fail("IllegalArgumentException expected but not thrown");
23758         } catch (final IllegalArgumentException ignore) {
23759         }
23760         assertNull(calibrator);
23761     }
23762 
23763     @Test
23764     public void testConstructor200() throws WrongSizeException {
23765         final Matrix ba = generateBa();
23766         final double biasX = ba.getElementAtIndex(0);
23767         final double biasY = ba.getElementAtIndex(1);
23768         final double biasZ = ba.getElementAtIndex(2);
23769 
23770         final Matrix ma = generateMaCommonAxis();
23771         final double sx = ma.getElementAt(0, 0);
23772         final double sy = ma.getElementAt(1, 1);
23773         final double sz = ma.getElementAt(2, 2);
23774         final double mxy = ma.getElementAt(0, 1);
23775         final double mxz = ma.getElementAt(0, 2);
23776         final double myx = ma.getElementAt(1, 0);
23777         final double myz = ma.getElementAt(1, 2);
23778         final double mzx = ma.getElementAt(2, 0);
23779         final double mzy = ma.getElementAt(2, 1);
23780 
23781         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23782         final double latitude = Math.toRadians(
23783                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23784         final double longitude = Math.toRadians(
23785                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23786         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23787         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23788         final NEDVelocity nedVelocity = new NEDVelocity();
23789         final ECEFPosition ecefPosition = new ECEFPosition();
23790         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23791         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23792                 ecefPosition, ecefVelocity);
23793         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23794                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23795         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23796 
23797         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
23798                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
23799                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23800                         myx, myz, mzx, mzy, this);
23801 
23802         // check default values
23803         assertEquals(calibrator.getBiasX(), biasX, 0.0);
23804         assertEquals(calibrator.getBiasY(), biasY, 0.0);
23805         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
23806         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
23807         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23808         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23809         final Acceleration bx2 = new Acceleration(0.0,
23810                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23811         calibrator.getBiasXAsAcceleration(bx2);
23812         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23813         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23814         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
23815         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23816         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23817         final Acceleration by2 = new Acceleration(0.0,
23818                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23819         calibrator.getBiasYAsAcceleration(by2);
23820         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23821         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23822         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
23823         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23824         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23825         final Acceleration bz2 = new Acceleration(0.0,
23826                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23827         calibrator.getBiasZAsAcceleration(bz2);
23828         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23829         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23830         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23831         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23832         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23833         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
23834         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
23835         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
23836         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
23837         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
23838         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
23839         final double[] bias1 = calibrator.getBias();
23840         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23841         final double[] bias2 = new double[3];
23842         calibrator.getBias(bias2);
23843         assertArrayEquals(bias1, bias2, 0.0);
23844         final Matrix b1 = calibrator.getBiasAsMatrix();
23845         assertEquals(b1, ba);
23846         final Matrix b2 = new Matrix(3, 1);
23847         calibrator.getBiasAsMatrix(b2);
23848         assertEquals(b1, b2);
23849         final Matrix ma1 = new Matrix(3, 3);
23850         ma1.setSubmatrix(0, 0,
23851                 2, 2,
23852                 new double[]{sx, myx, mzx,
23853                         mxy, sy, mzy,
23854                         mxz, myz, sz});
23855         assertEquals(calibrator.getInitialMa(), ma1);
23856         final Matrix ma2 = new Matrix(3, 3);
23857         calibrator.getInitialMa(ma2);
23858         assertEquals(ma1, ma2);
23859         assertNull(calibrator.getMeasurements());
23860         assertTrue(calibrator.isCommonAxisUsed());
23861         assertSame(calibrator.getListener(), this);
23862         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
23863         assertFalse(calibrator.isReady());
23864         assertFalse(calibrator.isRunning());
23865         assertNull(calibrator.getEstimatedMa());
23866         assertNull(calibrator.getEstimatedSx());
23867         assertNull(calibrator.getEstimatedSy());
23868         assertNull(calibrator.getEstimatedSz());
23869         assertNull(calibrator.getEstimatedMxy());
23870         assertNull(calibrator.getEstimatedMxz());
23871         assertNull(calibrator.getEstimatedMyx());
23872         assertNull(calibrator.getEstimatedMyz());
23873         assertNull(calibrator.getEstimatedMzx());
23874         assertNull(calibrator.getEstimatedMzy());
23875         assertNull(calibrator.getEstimatedCovariance());
23876         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23877         assertNotNull(calibrator.getGroundTruthGravityNorm());
23878         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23879         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23880         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23881                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23882         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23883         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23884         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23885 
23886         // Force IllegalArgumentException
23887         final Acceleration invalidGravityNorm = new Acceleration(
23888                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23889 
23890         calibrator = null;
23891         try {
23892             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
23893                     invalidGravityNorm, true, biasX, biasY, biasZ,
23894                     sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
23895                     this);
23896             fail("IllegalArgumentException expected but not thrown");
23897         } catch (final IllegalArgumentException ignore) {
23898         }
23899         assertNull(calibrator);
23900     }
23901 
23902     @Test
23903     public void testConstructor201() throws WrongSizeException {
23904         final Collection<StandardDeviationBodyKinematics> measurements =
23905                 Collections.emptyList();
23906 
23907         final Matrix ba = generateBa();
23908         final double biasX = ba.getElementAtIndex(0);
23909         final double biasY = ba.getElementAtIndex(1);
23910         final double biasZ = ba.getElementAtIndex(2);
23911 
23912         final Matrix ma = generateMaCommonAxis();
23913         final double sx = ma.getElementAt(0, 0);
23914         final double sy = ma.getElementAt(1, 1);
23915         final double sz = ma.getElementAt(2, 2);
23916         final double mxy = ma.getElementAt(0, 1);
23917         final double mxz = ma.getElementAt(0, 2);
23918         final double myx = ma.getElementAt(1, 0);
23919         final double myz = ma.getElementAt(1, 2);
23920         final double mzx = ma.getElementAt(2, 0);
23921         final double mzy = ma.getElementAt(2, 1);
23922 
23923         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23924         final double latitude = Math.toRadians(
23925                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23926         final double longitude = Math.toRadians(
23927                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23928         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23929         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23930         final NEDVelocity nedVelocity = new NEDVelocity();
23931         final ECEFPosition ecefPosition = new ECEFPosition();
23932         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23933         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23934                 ecefPosition, ecefVelocity);
23935         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23936                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23937         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23938 
23939         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
23940                 new KnownBiasAndGravityNormAccelerometerCalibrator(
23941                         gravityNorm, measurements,
23942                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23943                         myx, myz, mzx, mzy);
23944 
23945         // check default values
23946         assertEquals(calibrator.getBiasX(), biasX, 0.0);
23947         assertEquals(calibrator.getBiasY(), biasY, 0.0);
23948         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
23949         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
23950         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23951         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23952         final Acceleration bx2 = new Acceleration(0.0,
23953                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23954         calibrator.getBiasXAsAcceleration(bx2);
23955         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23956         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23957         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
23958         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23959         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23960         final Acceleration by2 = new Acceleration(0.0,
23961                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23962         calibrator.getBiasYAsAcceleration(by2);
23963         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23964         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23965         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
23966         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23967         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23968         final Acceleration bz2 = new Acceleration(0.0,
23969                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23970         calibrator.getBiasZAsAcceleration(bz2);
23971         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23972         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23973         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23974         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23975         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23976         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
23977         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
23978         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
23979         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
23980         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
23981         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
23982         final double[] bias1 = calibrator.getBias();
23983         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23984         final double[] bias2 = new double[3];
23985         calibrator.getBias(bias2);
23986         assertArrayEquals(bias1, bias2, 0.0);
23987         final Matrix b1 = calibrator.getBiasAsMatrix();
23988         assertEquals(b1, ba);
23989         final Matrix b2 = new Matrix(3, 1);
23990         calibrator.getBiasAsMatrix(b2);
23991         assertEquals(b1, b2);
23992         final Matrix ma1 = new Matrix(3, 3);
23993         ma1.setSubmatrix(0, 0,
23994                 2, 2,
23995                 new double[]{sx, myx, mzx,
23996                         mxy, sy, mzy,
23997                         mxz, myz, sz});
23998         assertEquals(calibrator.getInitialMa(), ma1);
23999         final Matrix ma2 = new Matrix(3, 3);
24000         calibrator.getInitialMa(ma2);
24001         assertEquals(ma1, ma2);
24002         assertSame(calibrator.getMeasurements(), measurements);
24003         assertTrue(calibrator.isCommonAxisUsed());
24004         assertNull(calibrator.getListener());
24005         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
24006         assertFalse(calibrator.isReady());
24007         assertFalse(calibrator.isRunning());
24008         assertNull(calibrator.getEstimatedMa());
24009         assertNull(calibrator.getEstimatedSx());
24010         assertNull(calibrator.getEstimatedSy());
24011         assertNull(calibrator.getEstimatedSz());
24012         assertNull(calibrator.getEstimatedMxy());
24013         assertNull(calibrator.getEstimatedMxz());
24014         assertNull(calibrator.getEstimatedMyx());
24015         assertNull(calibrator.getEstimatedMyz());
24016         assertNull(calibrator.getEstimatedMzx());
24017         assertNull(calibrator.getEstimatedMzy());
24018         assertNull(calibrator.getEstimatedCovariance());
24019         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24020         assertNotNull(calibrator.getGroundTruthGravityNorm());
24021         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24022         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24023         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24024                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24025         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24026         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24027         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24028 
24029         // Force IllegalArgumentException
24030         final Acceleration invalidGravityNorm = new Acceleration(
24031                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24032 
24033         calibrator = null;
24034         try {
24035             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
24036                     invalidGravityNorm, measurements,
24037                     true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
24038                     myx, myz, mzx, mzy);
24039             fail("IllegalArgumentException expected but not thrown");
24040         } catch (final IllegalArgumentException ignore) {
24041         }
24042         assertNull(calibrator);
24043     }
24044 
24045     @Test
24046     public void testConstructor202() throws WrongSizeException {
24047         final Collection<StandardDeviationBodyKinematics> measurements =
24048                 Collections.emptyList();
24049 
24050         final Matrix ba = generateBa();
24051         final double biasX = ba.getElementAtIndex(0);
24052         final double biasY = ba.getElementAtIndex(1);
24053         final double biasZ = ba.getElementAtIndex(2);
24054 
24055         final Matrix ma = generateMaCommonAxis();
24056         final double sx = ma.getElementAt(0, 0);
24057         final double sy = ma.getElementAt(1, 1);
24058         final double sz = ma.getElementAt(2, 2);
24059         final double mxy = ma.getElementAt(0, 1);
24060         final double mxz = ma.getElementAt(0, 2);
24061         final double myx = ma.getElementAt(1, 0);
24062         final double myz = ma.getElementAt(1, 2);
24063         final double mzx = ma.getElementAt(2, 0);
24064         final double mzy = ma.getElementAt(2, 1);
24065 
24066         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24067         final double latitude = Math.toRadians(
24068                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24069         final double longitude = Math.toRadians(
24070                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24071         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24072         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24073         final NEDVelocity nedVelocity = new NEDVelocity();
24074         final ECEFPosition ecefPosition = new ECEFPosition();
24075         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24076         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24077                 ecefPosition, ecefVelocity);
24078         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24079                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24080         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24081 
24082         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
24083                 new KnownBiasAndGravityNormAccelerometerCalibrator(
24084                         gravityNorm, measurements,
24085                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
24086                         myx, myz, mzx, mzy, this);
24087 
24088         // check default values
24089         assertEquals(calibrator.getBiasX(), biasX, 0.0);
24090         assertEquals(calibrator.getBiasY(), biasY, 0.0);
24091         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
24092         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
24093         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24094         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24095         final Acceleration bx2 = new Acceleration(0.0,
24096                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24097         calibrator.getBiasXAsAcceleration(bx2);
24098         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24099         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24100         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
24101         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24102         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24103         final Acceleration by2 = new Acceleration(0.0,
24104                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24105         calibrator.getBiasYAsAcceleration(by2);
24106         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24107         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24108         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
24109         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24110         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24111         final Acceleration bz2 = new Acceleration(0.0,
24112                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24113         calibrator.getBiasZAsAcceleration(bz2);
24114         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24115         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24116         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24117         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24118         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24119         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24120         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24121         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24122         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24123         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24124         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24125         final double[] bias1 = calibrator.getBias();
24126         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24127         final double[] bias2 = new double[3];
24128         calibrator.getBias(bias2);
24129         assertArrayEquals(bias1, bias2, 0.0);
24130         final Matrix b1 = calibrator.getBiasAsMatrix();
24131         assertEquals(b1, ba);
24132         final Matrix b2 = new Matrix(3, 1);
24133         calibrator.getBiasAsMatrix(b2);
24134         assertEquals(b1, b2);
24135         final Matrix ma1 = new Matrix(3, 3);
24136         ma1.setSubmatrix(0, 0,
24137                 2, 2,
24138                 new double[]{sx, myx, mzx,
24139                         mxy, sy, mzy,
24140                         mxz, myz, sz});
24141         assertEquals(calibrator.getInitialMa(), ma1);
24142         final Matrix ma2 = new Matrix(3, 3);
24143         calibrator.getInitialMa(ma2);
24144         assertEquals(ma1, ma2);
24145         assertSame(calibrator.getMeasurements(), measurements);
24146         assertTrue(calibrator.isCommonAxisUsed());
24147         assertSame(calibrator.getListener(), this);
24148         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
24149         assertFalse(calibrator.isReady());
24150         assertFalse(calibrator.isRunning());
24151         assertNull(calibrator.getEstimatedMa());
24152         assertNull(calibrator.getEstimatedSx());
24153         assertNull(calibrator.getEstimatedSy());
24154         assertNull(calibrator.getEstimatedSz());
24155         assertNull(calibrator.getEstimatedMxy());
24156         assertNull(calibrator.getEstimatedMxz());
24157         assertNull(calibrator.getEstimatedMyx());
24158         assertNull(calibrator.getEstimatedMyz());
24159         assertNull(calibrator.getEstimatedMzx());
24160         assertNull(calibrator.getEstimatedMzy());
24161         assertNull(calibrator.getEstimatedCovariance());
24162         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24163         assertNotNull(calibrator.getGroundTruthGravityNorm());
24164         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24165         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24166         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24167                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24168         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24169         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24170         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24171 
24172         // Force IllegalArgumentException
24173         final Acceleration invalidGravityNorm = new Acceleration(
24174                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24175 
24176         calibrator = null;
24177         try {
24178             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
24179                     invalidGravityNorm, measurements,
24180                     true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
24181                     myx, myz, mzx, mzy, this);
24182             fail("IllegalArgumentException expected but not thrown");
24183         } catch (final IllegalArgumentException ignore) {
24184         }
24185         assertNull(calibrator);
24186     }
24187 
24188     @Test
24189     public void testConstructor203() throws WrongSizeException {
24190         final Matrix ba = generateBa();
24191         final double biasX = ba.getElementAtIndex(0);
24192         final double biasY = ba.getElementAtIndex(1);
24193         final double biasZ = ba.getElementAtIndex(2);
24194 
24195         final Acceleration bx = new Acceleration(biasX,
24196                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24197         final Acceleration by = new Acceleration(biasY,
24198                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24199         final Acceleration bz = new Acceleration(biasZ,
24200                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24201 
24202         final Matrix ma = generateMaCommonAxis();
24203         final double sx = ma.getElementAt(0, 0);
24204         final double sy = ma.getElementAt(1, 1);
24205         final double sz = ma.getElementAt(2, 2);
24206         final double mxy = ma.getElementAt(0, 1);
24207         final double mxz = ma.getElementAt(0, 2);
24208         final double myx = ma.getElementAt(1, 0);
24209         final double myz = ma.getElementAt(1, 2);
24210         final double mzx = ma.getElementAt(2, 0);
24211         final double mzy = ma.getElementAt(2, 1);
24212 
24213         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24214         final double latitude = Math.toRadians(
24215                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24216         final double longitude = Math.toRadians(
24217                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24218         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24219         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24220         final NEDVelocity nedVelocity = new NEDVelocity();
24221         final ECEFPosition ecefPosition = new ECEFPosition();
24222         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24223         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24224                 ecefPosition, ecefVelocity);
24225         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24226                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24227         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24228 
24229         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
24230                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
24231                         bx, by, bz, sx, sy, sz, mxy, mxz,
24232                         myx, myz, mzx, mzy);
24233 
24234         // check default values
24235         assertEquals(calibrator.getBiasX(), biasX, 0.0);
24236         assertEquals(calibrator.getBiasY(), biasY, 0.0);
24237         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
24238         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
24239         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24240         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24241         final Acceleration bx2 = new Acceleration(0.0,
24242                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24243         calibrator.getBiasXAsAcceleration(bx2);
24244         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24245         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24246         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
24247         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24248         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24249         final Acceleration by2 = new Acceleration(0.0,
24250                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24251         calibrator.getBiasYAsAcceleration(by2);
24252         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24253         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24254         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
24255         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24256         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24257         final Acceleration bz2 = new Acceleration(0.0,
24258                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24259         calibrator.getBiasZAsAcceleration(bz2);
24260         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24261         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24262         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24263         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24264         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24265         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24266         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24267         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24268         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24269         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24270         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24271         final double[] bias1 = calibrator.getBias();
24272         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24273         final double[] bias2 = new double[3];
24274         calibrator.getBias(bias2);
24275         assertArrayEquals(bias1, bias2, 0.0);
24276         final Matrix b1 = calibrator.getBiasAsMatrix();
24277         assertEquals(b1, ba);
24278         final Matrix b2 = new Matrix(3, 1);
24279         calibrator.getBiasAsMatrix(b2);
24280         assertEquals(b1, b2);
24281         final Matrix ma1 = new Matrix(3, 3);
24282         ma1.setSubmatrix(0, 0,
24283                 2, 2,
24284                 new double[]{sx, myx, mzx,
24285                         mxy, sy, mzy,
24286                         mxz, myz, sz});
24287         assertEquals(calibrator.getInitialMa(), ma1);
24288         final Matrix ma2 = new Matrix(3, 3);
24289         calibrator.getInitialMa(ma2);
24290         assertEquals(ma1, ma2);
24291         assertNull(calibrator.getMeasurements());
24292         assertFalse(calibrator.isCommonAxisUsed());
24293         assertNull(calibrator.getListener());
24294         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24295         assertFalse(calibrator.isReady());
24296         assertFalse(calibrator.isRunning());
24297         assertNull(calibrator.getEstimatedMa());
24298         assertNull(calibrator.getEstimatedSx());
24299         assertNull(calibrator.getEstimatedSy());
24300         assertNull(calibrator.getEstimatedSz());
24301         assertNull(calibrator.getEstimatedMxy());
24302         assertNull(calibrator.getEstimatedMxz());
24303         assertNull(calibrator.getEstimatedMyx());
24304         assertNull(calibrator.getEstimatedMyz());
24305         assertNull(calibrator.getEstimatedMzx());
24306         assertNull(calibrator.getEstimatedMzy());
24307         assertNull(calibrator.getEstimatedCovariance());
24308         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24309         assertNotNull(calibrator.getGroundTruthGravityNorm());
24310         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24311         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24312         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24313                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24314         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24315         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24316         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24317 
24318         // Force IllegalArgumentException
24319         final Acceleration invalidGravityNorm = new Acceleration(
24320                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24321 
24322         calibrator = null;
24323         try {
24324             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
24325                     invalidGravityNorm, bx, by, bz, sx, sy, sz,
24326                     mxy, mxz, myx, myz, mzx, mzy);
24327             fail("IllegalArgumentException expected but not thrown");
24328         } catch (final IllegalArgumentException ignore) {
24329         }
24330         assertNull(calibrator);
24331     }
24332 
24333     @Test
24334     public void testConstructor204() throws WrongSizeException {
24335         final Matrix ba = generateBa();
24336         final double biasX = ba.getElementAtIndex(0);
24337         final double biasY = ba.getElementAtIndex(1);
24338         final double biasZ = ba.getElementAtIndex(2);
24339 
24340         final Acceleration bx = new Acceleration(biasX,
24341                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24342         final Acceleration by = new Acceleration(biasY,
24343                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24344         final Acceleration bz = new Acceleration(biasZ,
24345                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24346 
24347         final Matrix ma = generateMaCommonAxis();
24348         final double sx = ma.getElementAt(0, 0);
24349         final double sy = ma.getElementAt(1, 1);
24350         final double sz = ma.getElementAt(2, 2);
24351         final double mxy = ma.getElementAt(0, 1);
24352         final double mxz = ma.getElementAt(0, 2);
24353         final double myx = ma.getElementAt(1, 0);
24354         final double myz = ma.getElementAt(1, 2);
24355         final double mzx = ma.getElementAt(2, 0);
24356         final double mzy = ma.getElementAt(2, 1);
24357 
24358         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24359         final double latitude = Math.toRadians(
24360                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24361         final double longitude = Math.toRadians(
24362                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24363         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24364         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24365         final NEDVelocity nedVelocity = new NEDVelocity();
24366         final ECEFPosition ecefPosition = new ECEFPosition();
24367         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24368         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24369                 ecefPosition, ecefVelocity);
24370         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24371                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24372         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24373 
24374         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
24375                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
24376                         bx, by, bz, sx, sy, sz, mxy, mxz,
24377                         myx, myz, mzx, mzy, this);
24378 
24379         // check default values
24380         assertEquals(calibrator.getBiasX(), biasX, 0.0);
24381         assertEquals(calibrator.getBiasY(), biasY, 0.0);
24382         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
24383         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
24384         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24385         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24386         final Acceleration bx2 = new Acceleration(0.0,
24387                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24388         calibrator.getBiasXAsAcceleration(bx2);
24389         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24390         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24391         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
24392         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24393         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24394         final Acceleration by2 = new Acceleration(0.0,
24395                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24396         calibrator.getBiasYAsAcceleration(by2);
24397         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24398         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24399         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
24400         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24401         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24402         final Acceleration bz2 = new Acceleration(0.0,
24403                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24404         calibrator.getBiasZAsAcceleration(bz2);
24405         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24406         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24407         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24408         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24409         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24410         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24411         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24412         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24413         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24414         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24415         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24416         final double[] bias1 = calibrator.getBias();
24417         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24418         final double[] bias2 = new double[3];
24419         calibrator.getBias(bias2);
24420         assertArrayEquals(bias1, bias2, 0.0);
24421         final Matrix b1 = calibrator.getBiasAsMatrix();
24422         assertEquals(b1, ba);
24423         final Matrix b2 = new Matrix(3, 1);
24424         calibrator.getBiasAsMatrix(b2);
24425         assertEquals(b1, b2);
24426         final Matrix ma1 = new Matrix(3, 3);
24427         ma1.setSubmatrix(0, 0,
24428                 2, 2,
24429                 new double[]{sx, myx, mzx,
24430                         mxy, sy, mzy,
24431                         mxz, myz, sz});
24432         assertEquals(calibrator.getInitialMa(), ma1);
24433         final Matrix ma2 = new Matrix(3, 3);
24434         calibrator.getInitialMa(ma2);
24435         assertEquals(ma1, ma2);
24436         assertNull(calibrator.getMeasurements());
24437         assertFalse(calibrator.isCommonAxisUsed());
24438         assertSame(calibrator.getListener(), this);
24439         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24440         assertFalse(calibrator.isReady());
24441         assertFalse(calibrator.isRunning());
24442         assertNull(calibrator.getEstimatedMa());
24443         assertNull(calibrator.getEstimatedSx());
24444         assertNull(calibrator.getEstimatedSy());
24445         assertNull(calibrator.getEstimatedSz());
24446         assertNull(calibrator.getEstimatedMxy());
24447         assertNull(calibrator.getEstimatedMxz());
24448         assertNull(calibrator.getEstimatedMyx());
24449         assertNull(calibrator.getEstimatedMyz());
24450         assertNull(calibrator.getEstimatedMzx());
24451         assertNull(calibrator.getEstimatedMzy());
24452         assertNull(calibrator.getEstimatedCovariance());
24453         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24454         assertNotNull(calibrator.getGroundTruthGravityNorm());
24455         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24456         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24457         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24458                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24459         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24460         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24461         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24462 
24463         // Force IllegalArgumentException
24464         final Acceleration invalidGravityNorm = new Acceleration(
24465                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24466 
24467         calibrator = null;
24468         try {
24469             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
24470                     invalidGravityNorm, bx, by, bz, sx, sy, sz,
24471                     mxy, mxz, myx, myz, mzx, mzy, this);
24472             fail("IllegalArgumentException expected but not thrown");
24473         } catch (final IllegalArgumentException ignore) {
24474         }
24475         assertNull(calibrator);
24476     }
24477 
24478     @Test
24479     public void testConstructor205() throws WrongSizeException {
24480         final Collection<StandardDeviationBodyKinematics> measurements =
24481                 Collections.emptyList();
24482 
24483         final Matrix ba = generateBa();
24484         final double biasX = ba.getElementAtIndex(0);
24485         final double biasY = ba.getElementAtIndex(1);
24486         final double biasZ = ba.getElementAtIndex(2);
24487 
24488         final Acceleration bx = new Acceleration(biasX,
24489                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24490         final Acceleration by = new Acceleration(biasY,
24491                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24492         final Acceleration bz = new Acceleration(biasZ,
24493                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24494 
24495         final Matrix ma = generateMaCommonAxis();
24496         final double sx = ma.getElementAt(0, 0);
24497         final double sy = ma.getElementAt(1, 1);
24498         final double sz = ma.getElementAt(2, 2);
24499         final double mxy = ma.getElementAt(0, 1);
24500         final double mxz = ma.getElementAt(0, 2);
24501         final double myx = ma.getElementAt(1, 0);
24502         final double myz = ma.getElementAt(1, 2);
24503         final double mzx = ma.getElementAt(2, 0);
24504         final double mzy = ma.getElementAt(2, 1);
24505 
24506         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24507         final double latitude = Math.toRadians(
24508                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24509         final double longitude = Math.toRadians(
24510                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24511         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24512         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24513         final NEDVelocity nedVelocity = new NEDVelocity();
24514         final ECEFPosition ecefPosition = new ECEFPosition();
24515         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24516         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24517                 ecefPosition, ecefVelocity);
24518         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24519                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24520         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24521 
24522         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
24523                 new KnownBiasAndGravityNormAccelerometerCalibrator(
24524                         gravityNorm, measurements,
24525                         bx, by, bz, sx, sy, sz, mxy, mxz,
24526                         myx, myz, mzx, mzy);
24527 
24528         // check default values
24529         assertEquals(calibrator.getBiasX(), biasX, 0.0);
24530         assertEquals(calibrator.getBiasY(), biasY, 0.0);
24531         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
24532         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
24533         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24534         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24535         final Acceleration bx2 = new Acceleration(0.0,
24536                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24537         calibrator.getBiasXAsAcceleration(bx2);
24538         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24539         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24540         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
24541         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24542         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24543         final Acceleration by2 = new Acceleration(0.0,
24544                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24545         calibrator.getBiasYAsAcceleration(by2);
24546         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24547         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24548         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
24549         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24550         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24551         final Acceleration bz2 = new Acceleration(0.0,
24552                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24553         calibrator.getBiasZAsAcceleration(bz2);
24554         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24555         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24556         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24557         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24558         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24559         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24560         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24561         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24562         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24563         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24564         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24565         final double[] bias1 = calibrator.getBias();
24566         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24567         final double[] bias2 = new double[3];
24568         calibrator.getBias(bias2);
24569         assertArrayEquals(bias1, bias2, 0.0);
24570         final Matrix b1 = calibrator.getBiasAsMatrix();
24571         assertEquals(b1, ba);
24572         final Matrix b2 = new Matrix(3, 1);
24573         calibrator.getBiasAsMatrix(b2);
24574         assertEquals(b1, b2);
24575         final Matrix ma1 = new Matrix(3, 3);
24576         ma1.setSubmatrix(0, 0,
24577                 2, 2,
24578                 new double[]{sx, myx, mzx,
24579                         mxy, sy, mzy,
24580                         mxz, myz, sz});
24581         assertEquals(calibrator.getInitialMa(), ma1);
24582         final Matrix ma2 = new Matrix(3, 3);
24583         calibrator.getInitialMa(ma2);
24584         assertEquals(ma1, ma2);
24585         assertSame(calibrator.getMeasurements(), measurements);
24586         assertFalse(calibrator.isCommonAxisUsed());
24587         assertNull(calibrator.getListener());
24588         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24589         assertFalse(calibrator.isReady());
24590         assertFalse(calibrator.isRunning());
24591         assertNull(calibrator.getEstimatedMa());
24592         assertNull(calibrator.getEstimatedSx());
24593         assertNull(calibrator.getEstimatedSy());
24594         assertNull(calibrator.getEstimatedSz());
24595         assertNull(calibrator.getEstimatedMxy());
24596         assertNull(calibrator.getEstimatedMxz());
24597         assertNull(calibrator.getEstimatedMyx());
24598         assertNull(calibrator.getEstimatedMyz());
24599         assertNull(calibrator.getEstimatedMzx());
24600         assertNull(calibrator.getEstimatedMzy());
24601         assertNull(calibrator.getEstimatedCovariance());
24602         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24603         assertNotNull(calibrator.getGroundTruthGravityNorm());
24604         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24605         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24606         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24607                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24608         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24609         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24610         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24611 
24612         // Force IllegalArgumentException
24613         final Acceleration invalidGravityNorm = new Acceleration(
24614                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24615 
24616         calibrator = null;
24617         try {
24618             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
24619                     invalidGravityNorm, measurements,
24620                     bx, by, bz, sx, sy, sz, mxy, mxz,
24621                     myx, myz, mzx, mzy);
24622             fail("IllegalArgumentException expected but not thrown");
24623         } catch (final IllegalArgumentException ignore) {
24624         }
24625         assertNull(calibrator);
24626     }
24627 
24628     @Test
24629     public void testConstructor206() throws WrongSizeException {
24630         final Collection<StandardDeviationBodyKinematics> measurements =
24631                 Collections.emptyList();
24632 
24633         final Matrix ba = generateBa();
24634         final double biasX = ba.getElementAtIndex(0);
24635         final double biasY = ba.getElementAtIndex(1);
24636         final double biasZ = ba.getElementAtIndex(2);
24637 
24638         final Acceleration bx = new Acceleration(biasX,
24639                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24640         final Acceleration by = new Acceleration(biasY,
24641                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24642         final Acceleration bz = new Acceleration(biasZ,
24643                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24644 
24645         final Matrix ma = generateMaCommonAxis();
24646         final double sx = ma.getElementAt(0, 0);
24647         final double sy = ma.getElementAt(1, 1);
24648         final double sz = ma.getElementAt(2, 2);
24649         final double mxy = ma.getElementAt(0, 1);
24650         final double mxz = ma.getElementAt(0, 2);
24651         final double myx = ma.getElementAt(1, 0);
24652         final double myz = ma.getElementAt(1, 2);
24653         final double mzx = ma.getElementAt(2, 0);
24654         final double mzy = ma.getElementAt(2, 1);
24655 
24656         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24657         final double latitude = Math.toRadians(
24658                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24659         final double longitude = Math.toRadians(
24660                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24661         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24662         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24663         final NEDVelocity nedVelocity = new NEDVelocity();
24664         final ECEFPosition ecefPosition = new ECEFPosition();
24665         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24666         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24667                 ecefPosition, ecefVelocity);
24668         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24669                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24670         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24671 
24672         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
24673                 new KnownBiasAndGravityNormAccelerometerCalibrator(
24674                         gravityNorm, measurements,
24675                         bx, by, bz, sx, sy, sz, mxy, mxz,
24676                         myx, myz, mzx, mzy, this);
24677 
24678         // check default values
24679         assertEquals(calibrator.getBiasX(), biasX, 0.0);
24680         assertEquals(calibrator.getBiasY(), biasY, 0.0);
24681         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
24682         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
24683         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24684         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24685         final Acceleration bx2 = new Acceleration(0.0,
24686                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24687         calibrator.getBiasXAsAcceleration(bx2);
24688         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24689         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24690         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
24691         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24692         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24693         final Acceleration by2 = new Acceleration(0.0,
24694                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24695         calibrator.getBiasYAsAcceleration(by2);
24696         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24697         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24698         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
24699         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24700         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24701         final Acceleration bz2 = new Acceleration(0.0,
24702                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24703         calibrator.getBiasZAsAcceleration(bz2);
24704         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24705         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24706         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24707         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24708         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24709         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24710         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24711         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24712         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24713         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24714         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24715         final double[] bias1 = calibrator.getBias();
24716         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24717         final double[] bias2 = new double[3];
24718         calibrator.getBias(bias2);
24719         assertArrayEquals(bias1, bias2, 0.0);
24720         final Matrix b1 = calibrator.getBiasAsMatrix();
24721         assertEquals(b1, ba);
24722         final Matrix b2 = new Matrix(3, 1);
24723         calibrator.getBiasAsMatrix(b2);
24724         assertEquals(b1, b2);
24725         final Matrix ma1 = new Matrix(3, 3);
24726         ma1.setSubmatrix(0, 0,
24727                 2, 2,
24728                 new double[]{sx, myx, mzx,
24729                         mxy, sy, mzy,
24730                         mxz, myz, sz});
24731         assertEquals(calibrator.getInitialMa(), ma1);
24732         final Matrix ma2 = new Matrix(3, 3);
24733         calibrator.getInitialMa(ma2);
24734         assertEquals(ma1, ma2);
24735         assertSame(calibrator.getMeasurements(), measurements);
24736         assertFalse(calibrator.isCommonAxisUsed());
24737         assertSame(calibrator.getListener(), this);
24738         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24739         assertFalse(calibrator.isReady());
24740         assertFalse(calibrator.isRunning());
24741         assertNull(calibrator.getEstimatedMa());
24742         assertNull(calibrator.getEstimatedSx());
24743         assertNull(calibrator.getEstimatedSy());
24744         assertNull(calibrator.getEstimatedSz());
24745         assertNull(calibrator.getEstimatedMxy());
24746         assertNull(calibrator.getEstimatedMxz());
24747         assertNull(calibrator.getEstimatedMyx());
24748         assertNull(calibrator.getEstimatedMyz());
24749         assertNull(calibrator.getEstimatedMzx());
24750         assertNull(calibrator.getEstimatedMzy());
24751         assertNull(calibrator.getEstimatedCovariance());
24752         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24753         assertNotNull(calibrator.getGroundTruthGravityNorm());
24754         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24755         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24756         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24757                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24758         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24759         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24760         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24761 
24762         // Force IllegalArgumentException
24763         final Acceleration invalidGravityNorm = new Acceleration(
24764                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24765 
24766         calibrator = null;
24767         try {
24768             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
24769                     invalidGravityNorm, measurements,
24770                     bx, by, bz, sx, sy, sz, mxy, mxz,
24771                     myx, myz, mzx, mzy, this);
24772             fail("IllegalArgumentException expected but not thrown");
24773         } catch (final IllegalArgumentException ignore) {
24774         }
24775         assertNull(calibrator);
24776     }
24777 
24778     @Test
24779     public void testConstructor207() throws WrongSizeException {
24780         final Matrix ba = generateBa();
24781         final double biasX = ba.getElementAtIndex(0);
24782         final double biasY = ba.getElementAtIndex(1);
24783         final double biasZ = ba.getElementAtIndex(2);
24784 
24785         final Acceleration bx = new Acceleration(biasX,
24786                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24787         final Acceleration by = new Acceleration(biasY,
24788                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24789         final Acceleration bz = new Acceleration(biasZ,
24790                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24791 
24792         final Matrix ma = generateMaCommonAxis();
24793         final double sx = ma.getElementAt(0, 0);
24794         final double sy = ma.getElementAt(1, 1);
24795         final double sz = ma.getElementAt(2, 2);
24796         final double mxy = ma.getElementAt(0, 1);
24797         final double mxz = ma.getElementAt(0, 2);
24798         final double myx = ma.getElementAt(1, 0);
24799         final double myz = ma.getElementAt(1, 2);
24800         final double mzx = ma.getElementAt(2, 0);
24801         final double mzy = ma.getElementAt(2, 1);
24802 
24803         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24804         final double latitude = Math.toRadians(
24805                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24806         final double longitude = Math.toRadians(
24807                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24808         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24809         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24810         final NEDVelocity nedVelocity = new NEDVelocity();
24811         final ECEFPosition ecefPosition = new ECEFPosition();
24812         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24813         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24814                 ecefPosition, ecefVelocity);
24815         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24816                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24817         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24818 
24819         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
24820                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
24821                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
24822                         myx, myz, mzx, mzy);
24823 
24824         // check default values
24825         assertEquals(calibrator.getBiasX(), biasX, 0.0);
24826         assertEquals(calibrator.getBiasY(), biasY, 0.0);
24827         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
24828         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
24829         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24830         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24831         final Acceleration bx2 = new Acceleration(0.0,
24832                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24833         calibrator.getBiasXAsAcceleration(bx2);
24834         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24835         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24836         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
24837         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24838         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24839         final Acceleration by2 = new Acceleration(0.0,
24840                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24841         calibrator.getBiasYAsAcceleration(by2);
24842         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24843         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24844         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
24845         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24846         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24847         final Acceleration bz2 = new Acceleration(0.0,
24848                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24849         calibrator.getBiasZAsAcceleration(bz2);
24850         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24851         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24852         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24853         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24854         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24855         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24856         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24857         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24858         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24859         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24860         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24861         final double[] bias1 = calibrator.getBias();
24862         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24863         final double[] bias2 = new double[3];
24864         calibrator.getBias(bias2);
24865         assertArrayEquals(bias1, bias2, 0.0);
24866         final Matrix b1 = calibrator.getBiasAsMatrix();
24867         assertEquals(b1, ba);
24868         final Matrix b2 = new Matrix(3, 1);
24869         calibrator.getBiasAsMatrix(b2);
24870         assertEquals(b1, b2);
24871         final Matrix ma1 = new Matrix(3, 3);
24872         ma1.setSubmatrix(0, 0,
24873                 2, 2,
24874                 new double[]{sx, myx, mzx,
24875                         mxy, sy, mzy,
24876                         mxz, myz, sz});
24877         assertEquals(calibrator.getInitialMa(), ma1);
24878         final Matrix ma2 = new Matrix(3, 3);
24879         calibrator.getInitialMa(ma2);
24880         assertEquals(ma1, ma2);
24881         assertNull(calibrator.getMeasurements());
24882         assertTrue(calibrator.isCommonAxisUsed());
24883         assertNull(calibrator.getListener());
24884         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
24885         assertFalse(calibrator.isReady());
24886         assertFalse(calibrator.isRunning());
24887         assertNull(calibrator.getEstimatedMa());
24888         assertNull(calibrator.getEstimatedSx());
24889         assertNull(calibrator.getEstimatedSy());
24890         assertNull(calibrator.getEstimatedSz());
24891         assertNull(calibrator.getEstimatedMxy());
24892         assertNull(calibrator.getEstimatedMxz());
24893         assertNull(calibrator.getEstimatedMyx());
24894         assertNull(calibrator.getEstimatedMyz());
24895         assertNull(calibrator.getEstimatedMzx());
24896         assertNull(calibrator.getEstimatedMzy());
24897         assertNull(calibrator.getEstimatedCovariance());
24898         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24899         assertNotNull(calibrator.getGroundTruthGravityNorm());
24900         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24901         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24902         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24903                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24904         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24905         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24906         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24907 
24908         // Force IllegalArgumentException
24909         final Acceleration invalidGravityNorm = new Acceleration(
24910                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24911 
24912         calibrator = null;
24913         try {
24914             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
24915                     invalidGravityNorm, true,
24916                     bx, by, bz, sx, sy, sz, mxy, mxz,
24917                     myx, myz, mzx, mzy);
24918             fail("IllegalArgumentException expected but not thrown");
24919         } catch (final IllegalArgumentException ignore) {
24920         }
24921         assertNull(calibrator);
24922     }
24923 
24924     @Test
24925     public void testConstructor208() throws WrongSizeException {
24926         final Matrix ba = generateBa();
24927         final double biasX = ba.getElementAtIndex(0);
24928         final double biasY = ba.getElementAtIndex(1);
24929         final double biasZ = ba.getElementAtIndex(2);
24930 
24931         final Acceleration bx = new Acceleration(biasX,
24932                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24933         final Acceleration by = new Acceleration(biasY,
24934                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24935         final Acceleration bz = new Acceleration(biasZ,
24936                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24937 
24938         final Matrix ma = generateMaCommonAxis();
24939         final double sx = ma.getElementAt(0, 0);
24940         final double sy = ma.getElementAt(1, 1);
24941         final double sz = ma.getElementAt(2, 2);
24942         final double mxy = ma.getElementAt(0, 1);
24943         final double mxz = ma.getElementAt(0, 2);
24944         final double myx = ma.getElementAt(1, 0);
24945         final double myz = ma.getElementAt(1, 2);
24946         final double mzx = ma.getElementAt(2, 0);
24947         final double mzy = ma.getElementAt(2, 1);
24948 
24949         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24950         final double latitude = Math.toRadians(
24951                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24952         final double longitude = Math.toRadians(
24953                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24954         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24955         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24956         final NEDVelocity nedVelocity = new NEDVelocity();
24957         final ECEFPosition ecefPosition = new ECEFPosition();
24958         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24959         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24960                 ecefPosition, ecefVelocity);
24961         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24962                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24963         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24964 
24965         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
24966                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
24967                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
24968                         myx, myz, mzx, mzy, this);
24969 
24970         // check default values
24971         assertEquals(calibrator.getBiasX(), biasX, 0.0);
24972         assertEquals(calibrator.getBiasY(), biasY, 0.0);
24973         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
24974         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
24975         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24976         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24977         final Acceleration bx2 = new Acceleration(0.0,
24978                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24979         calibrator.getBiasXAsAcceleration(bx2);
24980         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24981         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24982         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
24983         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24984         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24985         final Acceleration by2 = new Acceleration(0.0,
24986                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24987         calibrator.getBiasYAsAcceleration(by2);
24988         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24989         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24990         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
24991         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24992         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24993         final Acceleration bz2 = new Acceleration(0.0,
24994                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24995         calibrator.getBiasZAsAcceleration(bz2);
24996         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24997         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24998         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24999         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25000         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25001         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25002         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25003         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25004         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25005         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25006         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25007         final double[] bias1 = calibrator.getBias();
25008         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25009         final double[] bias2 = new double[3];
25010         calibrator.getBias(bias2);
25011         assertArrayEquals(bias1, bias2, 0.0);
25012         final Matrix b1 = calibrator.getBiasAsMatrix();
25013         assertEquals(b1, ba);
25014         final Matrix b2 = new Matrix(3, 1);
25015         calibrator.getBiasAsMatrix(b2);
25016         assertEquals(b1, b2);
25017         final Matrix ma1 = new Matrix(3, 3);
25018         ma1.setSubmatrix(0, 0,
25019                 2, 2,
25020                 new double[]{sx, myx, mzx,
25021                         mxy, sy, mzy,
25022                         mxz, myz, sz});
25023         assertEquals(calibrator.getInitialMa(), ma1);
25024         final Matrix ma2 = new Matrix(3, 3);
25025         calibrator.getInitialMa(ma2);
25026         assertEquals(ma1, ma2);
25027         assertNull(calibrator.getMeasurements());
25028         assertTrue(calibrator.isCommonAxisUsed());
25029         assertSame(calibrator.getListener(), this);
25030         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
25031         assertFalse(calibrator.isReady());
25032         assertFalse(calibrator.isRunning());
25033         assertNull(calibrator.getEstimatedMa());
25034         assertNull(calibrator.getEstimatedSx());
25035         assertNull(calibrator.getEstimatedSy());
25036         assertNull(calibrator.getEstimatedSz());
25037         assertNull(calibrator.getEstimatedMxy());
25038         assertNull(calibrator.getEstimatedMxz());
25039         assertNull(calibrator.getEstimatedMyx());
25040         assertNull(calibrator.getEstimatedMyz());
25041         assertNull(calibrator.getEstimatedMzx());
25042         assertNull(calibrator.getEstimatedMzy());
25043         assertNull(calibrator.getEstimatedCovariance());
25044         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25045         assertNotNull(calibrator.getGroundTruthGravityNorm());
25046         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25047         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25048         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25049                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25050         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25051         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25052         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25053 
25054         // Force IllegalArgumentException
25055         final Acceleration invalidGravityNorm = new Acceleration(
25056                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25057 
25058         calibrator = null;
25059         try {
25060             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25061                     invalidGravityNorm, true,
25062                     bx, by, bz, sx, sy, sz, mxy, mxz,
25063                     myx, myz, mzx, mzy, this);
25064             fail("IllegalArgumentException expected but not thrown");
25065         } catch (final IllegalArgumentException ignore) {
25066         }
25067         assertNull(calibrator);
25068     }
25069 
25070     @Test
25071     public void testConstructor209() throws WrongSizeException {
25072         final Collection<StandardDeviationBodyKinematics> measurements =
25073                 Collections.emptyList();
25074 
25075         final Matrix ba = generateBa();
25076         final double biasX = ba.getElementAtIndex(0);
25077         final double biasY = ba.getElementAtIndex(1);
25078         final double biasZ = ba.getElementAtIndex(2);
25079 
25080         final Acceleration bx = new Acceleration(biasX,
25081                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25082         final Acceleration by = new Acceleration(biasY,
25083                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25084         final Acceleration bz = new Acceleration(biasZ,
25085                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25086 
25087         final Matrix ma = generateMaCommonAxis();
25088         final double sx = ma.getElementAt(0, 0);
25089         final double sy = ma.getElementAt(1, 1);
25090         final double sz = ma.getElementAt(2, 2);
25091         final double mxy = ma.getElementAt(0, 1);
25092         final double mxz = ma.getElementAt(0, 2);
25093         final double myx = ma.getElementAt(1, 0);
25094         final double myz = ma.getElementAt(1, 2);
25095         final double mzx = ma.getElementAt(2, 0);
25096         final double mzy = ma.getElementAt(2, 1);
25097 
25098         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25099         final double latitude = Math.toRadians(
25100                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25101         final double longitude = Math.toRadians(
25102                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25103         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25104         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25105         final NEDVelocity nedVelocity = new NEDVelocity();
25106         final ECEFPosition ecefPosition = new ECEFPosition();
25107         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25108         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25109                 ecefPosition, ecefVelocity);
25110         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25111                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25112         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25113 
25114         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
25115                 new KnownBiasAndGravityNormAccelerometerCalibrator(
25116                         gravityNorm, measurements,
25117                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
25118                         myx, myz, mzx, mzy);
25119 
25120         // check default values
25121         assertEquals(calibrator.getBiasX(), biasX, 0.0);
25122         assertEquals(calibrator.getBiasY(), biasY, 0.0);
25123         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
25124         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
25125         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25126         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25127         final Acceleration bx2 = new Acceleration(0.0,
25128                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25129         calibrator.getBiasXAsAcceleration(bx2);
25130         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25131         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25132         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
25133         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25134         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25135         final Acceleration by2 = new Acceleration(0.0,
25136                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25137         calibrator.getBiasYAsAcceleration(by2);
25138         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25139         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25140         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
25141         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25142         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25143         final Acceleration bz2 = new Acceleration(0.0,
25144                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25145         calibrator.getBiasZAsAcceleration(bz2);
25146         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25147         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25148         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25149         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25150         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25151         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25152         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25153         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25154         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25155         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25156         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25157         final double[] bias1 = calibrator.getBias();
25158         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25159         final double[] bias2 = new double[3];
25160         calibrator.getBias(bias2);
25161         assertArrayEquals(bias1, bias2, 0.0);
25162         final Matrix b1 = calibrator.getBiasAsMatrix();
25163         assertEquals(b1, ba);
25164         final Matrix b2 = new Matrix(3, 1);
25165         calibrator.getBiasAsMatrix(b2);
25166         assertEquals(b1, b2);
25167         final Matrix ma1 = new Matrix(3, 3);
25168         ma1.setSubmatrix(0, 0,
25169                 2, 2,
25170                 new double[]{sx, myx, mzx,
25171                         mxy, sy, mzy,
25172                         mxz, myz, sz});
25173         assertEquals(calibrator.getInitialMa(), ma1);
25174         final Matrix ma2 = new Matrix(3, 3);
25175         calibrator.getInitialMa(ma2);
25176         assertEquals(ma1, ma2);
25177         assertSame(calibrator.getMeasurements(), measurements);
25178         assertTrue(calibrator.isCommonAxisUsed());
25179         assertNull(calibrator.getListener());
25180         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
25181         assertFalse(calibrator.isReady());
25182         assertFalse(calibrator.isRunning());
25183         assertNull(calibrator.getEstimatedMa());
25184         assertNull(calibrator.getEstimatedSx());
25185         assertNull(calibrator.getEstimatedSy());
25186         assertNull(calibrator.getEstimatedSz());
25187         assertNull(calibrator.getEstimatedMxy());
25188         assertNull(calibrator.getEstimatedMxz());
25189         assertNull(calibrator.getEstimatedMyx());
25190         assertNull(calibrator.getEstimatedMyz());
25191         assertNull(calibrator.getEstimatedMzx());
25192         assertNull(calibrator.getEstimatedMzy());
25193         assertNull(calibrator.getEstimatedCovariance());
25194         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25195         assertNotNull(calibrator.getGroundTruthGravityNorm());
25196         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25197         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25198         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25199                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25200         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25201         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25202         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25203 
25204         // Force IllegalArgumentException
25205         final Acceleration invalidGravityNorm = new Acceleration(
25206                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25207 
25208         calibrator = null;
25209         try {
25210             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25211                     invalidGravityNorm, measurements,
25212                     true, bx, by, bz, sx, sy, sz, mxy, mxz,
25213                     myx, myz, mzx, mzy);
25214             fail("IllegalArgumentException expected but not thrown");
25215         } catch (final IllegalArgumentException ignore) {
25216         }
25217         assertNull(calibrator);
25218     }
25219 
25220     @Test
25221     public void testConstructor210() throws WrongSizeException {
25222         final Collection<StandardDeviationBodyKinematics> measurements =
25223                 Collections.emptyList();
25224 
25225         final Matrix ba = generateBa();
25226         final double biasX = ba.getElementAtIndex(0);
25227         final double biasY = ba.getElementAtIndex(1);
25228         final double biasZ = ba.getElementAtIndex(2);
25229 
25230         final Acceleration bx = new Acceleration(biasX,
25231                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25232         final Acceleration by = new Acceleration(biasY,
25233                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25234         final Acceleration bz = new Acceleration(biasZ,
25235                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25236 
25237         final Matrix ma = generateMaCommonAxis();
25238         final double sx = ma.getElementAt(0, 0);
25239         final double sy = ma.getElementAt(1, 1);
25240         final double sz = ma.getElementAt(2, 2);
25241         final double mxy = ma.getElementAt(0, 1);
25242         final double mxz = ma.getElementAt(0, 2);
25243         final double myx = ma.getElementAt(1, 0);
25244         final double myz = ma.getElementAt(1, 2);
25245         final double mzx = ma.getElementAt(2, 0);
25246         final double mzy = ma.getElementAt(2, 1);
25247 
25248         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25249         final double latitude = Math.toRadians(
25250                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25251         final double longitude = Math.toRadians(
25252                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25253         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25254         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25255         final NEDVelocity nedVelocity = new NEDVelocity();
25256         final ECEFPosition ecefPosition = new ECEFPosition();
25257         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25258         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25259                 ecefPosition, ecefVelocity);
25260         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25261                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25262         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25263 
25264         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
25265                 new KnownBiasAndGravityNormAccelerometerCalibrator(
25266                         gravityNorm, measurements,
25267                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
25268                         myx, myz, mzx, mzy, this);
25269 
25270         // check default values
25271         assertEquals(calibrator.getBiasX(), biasX, 0.0);
25272         assertEquals(calibrator.getBiasY(), biasY, 0.0);
25273         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
25274         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
25275         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25276         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25277         final Acceleration bx2 = new Acceleration(0.0,
25278                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25279         calibrator.getBiasXAsAcceleration(bx2);
25280         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25281         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25282         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
25283         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25284         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25285         final Acceleration by2 = new Acceleration(0.0,
25286                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25287         calibrator.getBiasYAsAcceleration(by2);
25288         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25289         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25290         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
25291         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25292         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25293         final Acceleration bz2 = new Acceleration(0.0,
25294                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25295         calibrator.getBiasZAsAcceleration(bz2);
25296         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25297         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25298         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25299         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25300         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25301         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25302         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25303         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25304         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25305         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25306         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25307         final double[] bias1 = calibrator.getBias();
25308         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25309         final double[] bias2 = new double[3];
25310         calibrator.getBias(bias2);
25311         assertArrayEquals(bias1, bias2, 0.0);
25312         final Matrix b1 = calibrator.getBiasAsMatrix();
25313         assertEquals(b1, ba);
25314         final Matrix b2 = new Matrix(3, 1);
25315         calibrator.getBiasAsMatrix(b2);
25316         assertEquals(b1, b2);
25317         final Matrix ma1 = new Matrix(3, 3);
25318         ma1.setSubmatrix(0, 0,
25319                 2, 2,
25320                 new double[]{sx, myx, mzx,
25321                         mxy, sy, mzy,
25322                         mxz, myz, sz});
25323         assertEquals(calibrator.getInitialMa(), ma1);
25324         final Matrix ma2 = new Matrix(3, 3);
25325         calibrator.getInitialMa(ma2);
25326         assertEquals(ma1, ma2);
25327         assertSame(calibrator.getMeasurements(), measurements);
25328         assertTrue(calibrator.isCommonAxisUsed());
25329         assertSame(calibrator.getListener(), this);
25330         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
25331         assertFalse(calibrator.isReady());
25332         assertFalse(calibrator.isRunning());
25333         assertNull(calibrator.getEstimatedMa());
25334         assertNull(calibrator.getEstimatedSx());
25335         assertNull(calibrator.getEstimatedSy());
25336         assertNull(calibrator.getEstimatedSz());
25337         assertNull(calibrator.getEstimatedMxy());
25338         assertNull(calibrator.getEstimatedMxz());
25339         assertNull(calibrator.getEstimatedMyx());
25340         assertNull(calibrator.getEstimatedMyz());
25341         assertNull(calibrator.getEstimatedMzx());
25342         assertNull(calibrator.getEstimatedMzy());
25343         assertNull(calibrator.getEstimatedCovariance());
25344         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25345         assertNotNull(calibrator.getGroundTruthGravityNorm());
25346         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25347         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25348         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25349                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25350         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25351         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25352         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25353 
25354         // Force IllegalArgumentException
25355         final Acceleration invalidGravityNorm = new Acceleration(
25356                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25357 
25358         calibrator = null;
25359         try {
25360             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25361                     invalidGravityNorm, measurements,
25362                     true, bx, by, bz, sx, sy, sz, mxy, mxz,
25363                     myx, myz, mzx, mzy, this);
25364             fail("IllegalArgumentException expected but not thrown");
25365         } catch (final IllegalArgumentException ignore) {
25366         }
25367         assertNull(calibrator);
25368     }
25369 
25370     @Test
25371     public void testConstructor211() throws WrongSizeException {
25372         final Matrix ba = generateBa();
25373         final double[] bias = ba.getBuffer();
25374         final double biasX = ba.getElementAtIndex(0);
25375         final double biasY = ba.getElementAtIndex(1);
25376         final double biasZ = ba.getElementAtIndex(2);
25377 
25378         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25379         final double latitude = Math.toRadians(
25380                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25381         final double longitude = Math.toRadians(
25382                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25383         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25384         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25385         final NEDVelocity nedVelocity = new NEDVelocity();
25386         final ECEFPosition ecefPosition = new ECEFPosition();
25387         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25388         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25389                 ecefPosition, ecefVelocity);
25390         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25391                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25392         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25393 
25394         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
25395                 new KnownBiasAndGravityNormAccelerometerCalibrator(
25396                         gravityNorm, bias);
25397 
25398         // check default values
25399         assertEquals(calibrator.getBiasX(), biasX, 0.0);
25400         assertEquals(calibrator.getBiasY(), biasY, 0.0);
25401         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
25402         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
25403         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25404         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25405         final Acceleration bx2 = new Acceleration(0.0,
25406                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25407         calibrator.getBiasXAsAcceleration(bx2);
25408         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25409         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25410         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
25411         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25412         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25413         final Acceleration by2 = new Acceleration(0.0,
25414                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25415         calibrator.getBiasYAsAcceleration(by2);
25416         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25417         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25418         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
25419         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25420         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25421         final Acceleration bz2 = new Acceleration(0.0,
25422                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25423         calibrator.getBiasZAsAcceleration(bz2);
25424         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25425         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25426         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
25427         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
25428         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
25429         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25430         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25431         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25432         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25433         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25434         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25435         final double[] bias1 = calibrator.getBias();
25436         assertArrayEquals(bias1, bias, 0.0);
25437         final double[] bias2 = new double[3];
25438         calibrator.getBias(bias2);
25439         assertArrayEquals(bias1, bias2, 0.0);
25440         final Matrix b1 = calibrator.getBiasAsMatrix();
25441         assertEquals(b1, ba);
25442         final Matrix b2 = new Matrix(3, 1);
25443         calibrator.getBiasAsMatrix(b2);
25444         assertEquals(b1, b2);
25445         final Matrix ma1 = calibrator.getInitialMa();
25446         assertEquals(ma1, new Matrix(3, 3));
25447         final Matrix ma2 = new Matrix(3, 3);
25448         calibrator.getInitialMa(ma2);
25449         assertEquals(ma1, ma2);
25450         assertNull(calibrator.getMeasurements());
25451         assertFalse(calibrator.isCommonAxisUsed());
25452         assertNull(calibrator.getListener());
25453         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25454         assertFalse(calibrator.isReady());
25455         assertFalse(calibrator.isRunning());
25456         assertNull(calibrator.getEstimatedMa());
25457         assertNull(calibrator.getEstimatedSx());
25458         assertNull(calibrator.getEstimatedSy());
25459         assertNull(calibrator.getEstimatedSz());
25460         assertNull(calibrator.getEstimatedMxy());
25461         assertNull(calibrator.getEstimatedMxz());
25462         assertNull(calibrator.getEstimatedMyx());
25463         assertNull(calibrator.getEstimatedMyz());
25464         assertNull(calibrator.getEstimatedMzx());
25465         assertNull(calibrator.getEstimatedMzy());
25466         assertNull(calibrator.getEstimatedCovariance());
25467         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25468         assertNotNull(calibrator.getGroundTruthGravityNorm());
25469         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25470         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25471         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25472                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25473         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25474         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25475         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25476 
25477         // Force IllegalArgumentException
25478         final Acceleration invalidGravityNorm = new Acceleration(
25479                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25480 
25481         calibrator = null;
25482         try {
25483             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25484                     invalidGravityNorm, bias);
25485             fail("IllegalArgumentException expected but not thrown");
25486         } catch (final IllegalArgumentException ignore) {
25487         }
25488         try {
25489             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25490                     gravityNorm, new double[1]);
25491             fail("IllegalArgumentException expected but not thrown");
25492         } catch (final IllegalArgumentException ignore) {
25493         }
25494         assertNull(calibrator);
25495     }
25496 
25497     @Test
25498     public void testConstructor212() throws WrongSizeException {
25499         final Matrix ba = generateBa();
25500         final double[] bias = ba.getBuffer();
25501         final double biasX = ba.getElementAtIndex(0);
25502         final double biasY = ba.getElementAtIndex(1);
25503         final double biasZ = ba.getElementAtIndex(2);
25504 
25505         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25506         final double latitude = Math.toRadians(
25507                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25508         final double longitude = Math.toRadians(
25509                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25510         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25511         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25512         final NEDVelocity nedVelocity = new NEDVelocity();
25513         final ECEFPosition ecefPosition = new ECEFPosition();
25514         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25515         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25516                 ecefPosition, ecefVelocity);
25517         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25518                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25519         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25520 
25521         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
25522                 new KnownBiasAndGravityNormAccelerometerCalibrator(
25523                         gravityNorm, bias, this);
25524 
25525         // check default values
25526         assertEquals(calibrator.getBiasX(), biasX, 0.0);
25527         assertEquals(calibrator.getBiasY(), biasY, 0.0);
25528         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
25529         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
25530         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25531         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25532         final Acceleration bx2 = new Acceleration(0.0,
25533                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25534         calibrator.getBiasXAsAcceleration(bx2);
25535         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25536         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25537         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
25538         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25539         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25540         final Acceleration by2 = new Acceleration(0.0,
25541                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25542         calibrator.getBiasYAsAcceleration(by2);
25543         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25544         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25545         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
25546         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25547         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25548         final Acceleration bz2 = new Acceleration(0.0,
25549                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25550         calibrator.getBiasZAsAcceleration(bz2);
25551         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25552         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25553         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
25554         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
25555         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
25556         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25557         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25558         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25559         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25560         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25561         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25562         final double[] bias1 = calibrator.getBias();
25563         assertArrayEquals(bias1, bias, 0.0);
25564         final double[] bias2 = new double[3];
25565         calibrator.getBias(bias2);
25566         assertArrayEquals(bias1, bias2, 0.0);
25567         final Matrix b1 = calibrator.getBiasAsMatrix();
25568         assertEquals(b1, ba);
25569         final Matrix b2 = new Matrix(3, 1);
25570         calibrator.getBiasAsMatrix(b2);
25571         assertEquals(b1, b2);
25572         final Matrix ma1 = calibrator.getInitialMa();
25573         assertEquals(ma1, new Matrix(3, 3));
25574         final Matrix ma2 = new Matrix(3, 3);
25575         calibrator.getInitialMa(ma2);
25576         assertEquals(ma1, ma2);
25577         assertNull(calibrator.getMeasurements());
25578         assertFalse(calibrator.isCommonAxisUsed());
25579         assertSame(calibrator.getListener(), this);
25580         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25581         assertFalse(calibrator.isReady());
25582         assertFalse(calibrator.isRunning());
25583         assertNull(calibrator.getEstimatedMa());
25584         assertNull(calibrator.getEstimatedSx());
25585         assertNull(calibrator.getEstimatedSy());
25586         assertNull(calibrator.getEstimatedSz());
25587         assertNull(calibrator.getEstimatedMxy());
25588         assertNull(calibrator.getEstimatedMxz());
25589         assertNull(calibrator.getEstimatedMyx());
25590         assertNull(calibrator.getEstimatedMyz());
25591         assertNull(calibrator.getEstimatedMzx());
25592         assertNull(calibrator.getEstimatedMzy());
25593         assertNull(calibrator.getEstimatedCovariance());
25594         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25595         assertNotNull(calibrator.getGroundTruthGravityNorm());
25596         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25597         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25598         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25599                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25600         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25601         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25602         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25603 
25604         // Force IllegalArgumentException
25605         final Acceleration invalidGravityNorm = new Acceleration(
25606                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25607 
25608         calibrator = null;
25609         try {
25610             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25611                     invalidGravityNorm, bias, this);
25612             fail("IllegalArgumentException expected but not thrown");
25613         } catch (final IllegalArgumentException ignore) {
25614         }
25615         try {
25616             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25617                     gravityNorm, new double[1], this);
25618             fail("IllegalArgumentException expected but not thrown");
25619         } catch (final IllegalArgumentException ignore) {
25620         }
25621         assertNull(calibrator);
25622     }
25623 
25624     @Test
25625     public void testConstructor213() throws WrongSizeException {
25626         final Collection<StandardDeviationBodyKinematics> measurements =
25627                 Collections.emptyList();
25628 
25629         final Matrix ba = generateBa();
25630         final double[] bias = ba.getBuffer();
25631         final double biasX = ba.getElementAtIndex(0);
25632         final double biasY = ba.getElementAtIndex(1);
25633         final double biasZ = ba.getElementAtIndex(2);
25634 
25635         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25636         final double latitude = Math.toRadians(
25637                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25638         final double longitude = Math.toRadians(
25639                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25640         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25641         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25642         final NEDVelocity nedVelocity = new NEDVelocity();
25643         final ECEFPosition ecefPosition = new ECEFPosition();
25644         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25645         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25646                 ecefPosition, ecefVelocity);
25647         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25648                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25649         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25650 
25651         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
25652                 new KnownBiasAndGravityNormAccelerometerCalibrator(
25653                         gravityNorm, measurements, bias);
25654 
25655         // check default values
25656         assertEquals(calibrator.getBiasX(), biasX, 0.0);
25657         assertEquals(calibrator.getBiasY(), biasY, 0.0);
25658         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
25659         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
25660         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25661         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25662         final Acceleration bx2 = new Acceleration(0.0,
25663                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25664         calibrator.getBiasXAsAcceleration(bx2);
25665         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25666         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25667         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
25668         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25669         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25670         final Acceleration by2 = new Acceleration(0.0,
25671                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25672         calibrator.getBiasYAsAcceleration(by2);
25673         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25674         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25675         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
25676         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25677         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25678         final Acceleration bz2 = new Acceleration(0.0,
25679                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25680         calibrator.getBiasZAsAcceleration(bz2);
25681         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25682         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25683         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
25684         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
25685         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
25686         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25687         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25688         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25689         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25690         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25691         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25692         final double[] bias1 = calibrator.getBias();
25693         assertArrayEquals(bias1, bias, 0.0);
25694         final double[] bias2 = new double[3];
25695         calibrator.getBias(bias2);
25696         assertArrayEquals(bias1, bias2, 0.0);
25697         final Matrix b1 = calibrator.getBiasAsMatrix();
25698         assertEquals(b1, ba);
25699         final Matrix b2 = new Matrix(3, 1);
25700         calibrator.getBiasAsMatrix(b2);
25701         assertEquals(b1, b2);
25702         final Matrix ma1 = calibrator.getInitialMa();
25703         assertEquals(ma1, new Matrix(3, 3));
25704         final Matrix ma2 = new Matrix(3, 3);
25705         calibrator.getInitialMa(ma2);
25706         assertEquals(ma1, ma2);
25707         assertSame(calibrator.getMeasurements(), measurements);
25708         assertFalse(calibrator.isCommonAxisUsed());
25709         assertNull(calibrator.getListener());
25710         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25711         assertFalse(calibrator.isReady());
25712         assertFalse(calibrator.isRunning());
25713         assertNull(calibrator.getEstimatedMa());
25714         assertNull(calibrator.getEstimatedSx());
25715         assertNull(calibrator.getEstimatedSy());
25716         assertNull(calibrator.getEstimatedSz());
25717         assertNull(calibrator.getEstimatedMxy());
25718         assertNull(calibrator.getEstimatedMxz());
25719         assertNull(calibrator.getEstimatedMyx());
25720         assertNull(calibrator.getEstimatedMyz());
25721         assertNull(calibrator.getEstimatedMzx());
25722         assertNull(calibrator.getEstimatedMzy());
25723         assertNull(calibrator.getEstimatedCovariance());
25724         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25725         assertNotNull(calibrator.getGroundTruthGravityNorm());
25726         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25727         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25728         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25729                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25730         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25731         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25732         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25733 
25734         // Force IllegalArgumentException
25735         final Acceleration invalidGravityNorm = new Acceleration(
25736                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25737 
25738         calibrator = null;
25739         try {
25740             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25741                     invalidGravityNorm, measurements, bias);
25742             fail("IllegalArgumentException expected but not thrown");
25743         } catch (final IllegalArgumentException ignore) {
25744         }
25745         try {
25746             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25747                     gravityNorm, measurements, new double[1]);
25748             fail("IllegalArgumentException expected but not thrown");
25749         } catch (final IllegalArgumentException ignore) {
25750         }
25751         assertNull(calibrator);
25752     }
25753 
25754     @Test
25755     public void testConstructor214() throws WrongSizeException {
25756         final Collection<StandardDeviationBodyKinematics> measurements =
25757                 Collections.emptyList();
25758 
25759         final Matrix ba = generateBa();
25760         final double[] bias = ba.getBuffer();
25761         final double biasX = ba.getElementAtIndex(0);
25762         final double biasY = ba.getElementAtIndex(1);
25763         final double biasZ = ba.getElementAtIndex(2);
25764 
25765         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25766         final double latitude = Math.toRadians(
25767                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25768         final double longitude = Math.toRadians(
25769                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25770         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25771         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25772         final NEDVelocity nedVelocity = new NEDVelocity();
25773         final ECEFPosition ecefPosition = new ECEFPosition();
25774         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25775         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25776                 ecefPosition, ecefVelocity);
25777         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25778                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25779         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25780 
25781         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
25782                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
25783                         measurements, bias, this);
25784 
25785         // check default values
25786         assertEquals(calibrator.getBiasX(), biasX, 0.0);
25787         assertEquals(calibrator.getBiasY(), biasY, 0.0);
25788         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
25789         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
25790         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25791         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25792         final Acceleration bx2 = new Acceleration(0.0,
25793                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25794         calibrator.getBiasXAsAcceleration(bx2);
25795         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25796         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25797         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
25798         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25799         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25800         final Acceleration by2 = new Acceleration(0.0,
25801                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25802         calibrator.getBiasYAsAcceleration(by2);
25803         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25804         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25805         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
25806         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25807         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25808         final Acceleration bz2 = new Acceleration(0.0,
25809                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25810         calibrator.getBiasZAsAcceleration(bz2);
25811         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25812         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25813         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
25814         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
25815         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
25816         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25817         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25818         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25819         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25820         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25821         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25822         final double[] bias1 = calibrator.getBias();
25823         assertArrayEquals(bias1, bias, 0.0);
25824         final double[] bias2 = new double[3];
25825         calibrator.getBias(bias2);
25826         assertArrayEquals(bias1, bias2, 0.0);
25827         final Matrix b1 = calibrator.getBiasAsMatrix();
25828         assertEquals(b1, ba);
25829         final Matrix b2 = new Matrix(3, 1);
25830         calibrator.getBiasAsMatrix(b2);
25831         assertEquals(b1, b2);
25832         final Matrix ma1 = calibrator.getInitialMa();
25833         assertEquals(ma1, new Matrix(3, 3));
25834         final Matrix ma2 = new Matrix(3, 3);
25835         calibrator.getInitialMa(ma2);
25836         assertEquals(ma1, ma2);
25837         assertSame(calibrator.getMeasurements(), measurements);
25838         assertFalse(calibrator.isCommonAxisUsed());
25839         assertSame(calibrator.getListener(), this);
25840         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25841         assertFalse(calibrator.isReady());
25842         assertFalse(calibrator.isRunning());
25843         assertNull(calibrator.getEstimatedMa());
25844         assertNull(calibrator.getEstimatedSx());
25845         assertNull(calibrator.getEstimatedSy());
25846         assertNull(calibrator.getEstimatedSz());
25847         assertNull(calibrator.getEstimatedMxy());
25848         assertNull(calibrator.getEstimatedMxz());
25849         assertNull(calibrator.getEstimatedMyx());
25850         assertNull(calibrator.getEstimatedMyz());
25851         assertNull(calibrator.getEstimatedMzx());
25852         assertNull(calibrator.getEstimatedMzy());
25853         assertNull(calibrator.getEstimatedCovariance());
25854         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25855         assertNotNull(calibrator.getGroundTruthGravityNorm());
25856         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25857         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25858         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25859                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25860         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25861         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25862         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25863 
25864         // Force IllegalArgumentException
25865         final Acceleration invalidGravityNorm = new Acceleration(
25866                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25867 
25868         calibrator = null;
25869         try {
25870             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25871                     invalidGravityNorm, measurements, bias, this);
25872             fail("IllegalArgumentException expected but not thrown");
25873         } catch (final IllegalArgumentException ignore) {
25874         }
25875         try {
25876             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25877                     gravityNorm, measurements,
25878                     new double[1], this);
25879             fail("IllegalArgumentException expected but not thrown");
25880         } catch (final IllegalArgumentException ignore) {
25881         }
25882         assertNull(calibrator);
25883     }
25884 
25885     @Test
25886     public void testConstructor215() throws WrongSizeException {
25887         final Matrix ba = generateBa();
25888         final double[] bias = ba.getBuffer();
25889         final double biasX = ba.getElementAtIndex(0);
25890         final double biasY = ba.getElementAtIndex(1);
25891         final double biasZ = ba.getElementAtIndex(2);
25892 
25893         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25894         final double latitude = Math.toRadians(
25895                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25896         final double longitude = Math.toRadians(
25897                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25898         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25899         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25900         final NEDVelocity nedVelocity = new NEDVelocity();
25901         final ECEFPosition ecefPosition = new ECEFPosition();
25902         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25903         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25904                 ecefPosition, ecefVelocity);
25905         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25906                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25907         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25908 
25909         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
25910                 new KnownBiasAndGravityNormAccelerometerCalibrator(
25911                         gravityNorm, true, bias);
25912 
25913         // check default values
25914         assertEquals(calibrator.getBiasX(), biasX, 0.0);
25915         assertEquals(calibrator.getBiasY(), biasY, 0.0);
25916         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
25917         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
25918         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25919         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25920         final Acceleration bx2 = new Acceleration(0.0,
25921                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25922         calibrator.getBiasXAsAcceleration(bx2);
25923         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25924         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25925         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
25926         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25927         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25928         final Acceleration by2 = new Acceleration(0.0,
25929                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25930         calibrator.getBiasYAsAcceleration(by2);
25931         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25932         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25933         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
25934         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25935         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25936         final Acceleration bz2 = new Acceleration(0.0,
25937                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25938         calibrator.getBiasZAsAcceleration(bz2);
25939         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25940         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25941         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
25942         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
25943         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
25944         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25945         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25946         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25947         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25948         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25949         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25950         final double[] bias1 = calibrator.getBias();
25951         assertArrayEquals(bias1, bias, 0.0);
25952         final double[] bias2 = new double[3];
25953         calibrator.getBias(bias2);
25954         assertArrayEquals(bias1, bias2, 0.0);
25955         final Matrix b1 = calibrator.getBiasAsMatrix();
25956         assertEquals(b1, ba);
25957         final Matrix b2 = new Matrix(3, 1);
25958         calibrator.getBiasAsMatrix(b2);
25959         assertEquals(b1, b2);
25960         final Matrix ma1 = calibrator.getInitialMa();
25961         assertEquals(ma1, new Matrix(3, 3));
25962         final Matrix ma2 = new Matrix(3, 3);
25963         calibrator.getInitialMa(ma2);
25964         assertEquals(ma1, ma2);
25965         assertNull(calibrator.getMeasurements());
25966         assertTrue(calibrator.isCommonAxisUsed());
25967         assertNull(calibrator.getListener());
25968         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
25969         assertFalse(calibrator.isReady());
25970         assertFalse(calibrator.isRunning());
25971         assertNull(calibrator.getEstimatedMa());
25972         assertNull(calibrator.getEstimatedSx());
25973         assertNull(calibrator.getEstimatedSy());
25974         assertNull(calibrator.getEstimatedSz());
25975         assertNull(calibrator.getEstimatedMxy());
25976         assertNull(calibrator.getEstimatedMxz());
25977         assertNull(calibrator.getEstimatedMyx());
25978         assertNull(calibrator.getEstimatedMyz());
25979         assertNull(calibrator.getEstimatedMzx());
25980         assertNull(calibrator.getEstimatedMzy());
25981         assertNull(calibrator.getEstimatedCovariance());
25982         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25983         assertNotNull(calibrator.getGroundTruthGravityNorm());
25984         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25985         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25986         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25987                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25988         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25989         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25990         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25991 
25992         // Force IllegalArgumentException
25993         final Acceleration invalidGravityNorm = new Acceleration(
25994                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25995 
25996         calibrator = null;
25997         try {
25998             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
25999                     invalidGravityNorm, true, bias);
26000             fail("IllegalArgumentException expected but not thrown");
26001         } catch (final IllegalArgumentException ignore) {
26002         }
26003         try {
26004             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26005                     gravityNorm, true, new double[1]);
26006             fail("IllegalArgumentException expected but not thrown");
26007         } catch (final IllegalArgumentException ignore) {
26008         }
26009         assertNull(calibrator);
26010     }
26011 
26012     @Test
26013     public void testConstructor216() throws WrongSizeException {
26014         final Matrix ba = generateBa();
26015         final double[] bias = ba.getBuffer();
26016         final double biasX = ba.getElementAtIndex(0);
26017         final double biasY = ba.getElementAtIndex(1);
26018         final double biasZ = ba.getElementAtIndex(2);
26019 
26020         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26021         final double latitude = Math.toRadians(
26022                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26023         final double longitude = Math.toRadians(
26024                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26025         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26026         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26027         final NEDVelocity nedVelocity = new NEDVelocity();
26028         final ECEFPosition ecefPosition = new ECEFPosition();
26029         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26030         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26031                 ecefPosition, ecefVelocity);
26032         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26033                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26034         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26035 
26036         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
26037                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
26038                         true, bias, this);
26039 
26040         // check default values
26041         assertEquals(calibrator.getBiasX(), biasX, 0.0);
26042         assertEquals(calibrator.getBiasY(), biasY, 0.0);
26043         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
26044         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
26045         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26046         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26047         final Acceleration bx2 = new Acceleration(0.0,
26048                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26049         calibrator.getBiasXAsAcceleration(bx2);
26050         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26051         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26052         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
26053         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26054         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26055         final Acceleration by2 = new Acceleration(0.0,
26056                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26057         calibrator.getBiasYAsAcceleration(by2);
26058         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26059         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26060         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
26061         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26062         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26063         final Acceleration bz2 = new Acceleration(0.0,
26064                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26065         calibrator.getBiasZAsAcceleration(bz2);
26066         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26067         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26068         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26069         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26070         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26071         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26072         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26073         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26074         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26075         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26076         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26077         final double[] bias1 = calibrator.getBias();
26078         assertArrayEquals(bias1, bias, 0.0);
26079         final double[] bias2 = new double[3];
26080         calibrator.getBias(bias2);
26081         assertArrayEquals(bias1, bias2, 0.0);
26082         final Matrix b1 = calibrator.getBiasAsMatrix();
26083         assertEquals(b1, ba);
26084         final Matrix b2 = new Matrix(3, 1);
26085         calibrator.getBiasAsMatrix(b2);
26086         assertEquals(b1, b2);
26087         final Matrix ma1 = calibrator.getInitialMa();
26088         assertEquals(ma1, new Matrix(3, 3));
26089         final Matrix ma2 = new Matrix(3, 3);
26090         calibrator.getInitialMa(ma2);
26091         assertEquals(ma1, ma2);
26092         assertNull(calibrator.getMeasurements());
26093         assertTrue(calibrator.isCommonAxisUsed());
26094         assertSame(calibrator.getListener(), this);
26095         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
26096         assertFalse(calibrator.isReady());
26097         assertFalse(calibrator.isRunning());
26098         assertNull(calibrator.getEstimatedMa());
26099         assertNull(calibrator.getEstimatedSx());
26100         assertNull(calibrator.getEstimatedSy());
26101         assertNull(calibrator.getEstimatedSz());
26102         assertNull(calibrator.getEstimatedMxy());
26103         assertNull(calibrator.getEstimatedMxz());
26104         assertNull(calibrator.getEstimatedMyx());
26105         assertNull(calibrator.getEstimatedMyz());
26106         assertNull(calibrator.getEstimatedMzx());
26107         assertNull(calibrator.getEstimatedMzy());
26108         assertNull(calibrator.getEstimatedCovariance());
26109         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26110         assertNotNull(calibrator.getGroundTruthGravityNorm());
26111         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26112         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26113         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26114                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26115         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26116         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26117         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26118 
26119         // Force IllegalArgumentException
26120         final Acceleration invalidGravityNorm = new Acceleration(
26121                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26122 
26123         calibrator = null;
26124         try {
26125             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26126                     invalidGravityNorm, true,
26127                     bias, this);
26128             fail("IllegalArgumentException expected but not thrown");
26129         } catch (final IllegalArgumentException ignore) {
26130         }
26131         try {
26132             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26133                     gravityNorm, true,
26134                     new double[1], this);
26135             fail("IllegalArgumentException expected but not thrown");
26136         } catch (final IllegalArgumentException ignore) {
26137         }
26138         assertNull(calibrator);
26139     }
26140 
26141     @Test
26142     public void testConstructor217() throws WrongSizeException {
26143         final Collection<StandardDeviationBodyKinematics> measurements =
26144                 Collections.emptyList();
26145 
26146         final Matrix ba = generateBa();
26147         final double[] bias = ba.getBuffer();
26148         final double biasX = ba.getElementAtIndex(0);
26149         final double biasY = ba.getElementAtIndex(1);
26150         final double biasZ = ba.getElementAtIndex(2);
26151 
26152         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26153         final double latitude = Math.toRadians(
26154                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26155         final double longitude = Math.toRadians(
26156                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26157         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26158         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26159         final NEDVelocity nedVelocity = new NEDVelocity();
26160         final ECEFPosition ecefPosition = new ECEFPosition();
26161         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26162         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26163                 ecefPosition, ecefVelocity);
26164         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26165                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26166         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26167 
26168         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
26169                 new KnownBiasAndGravityNormAccelerometerCalibrator(
26170                         gravityNorm, measurements,
26171                         true, bias);
26172 
26173         // check default values
26174         assertEquals(calibrator.getBiasX(), biasX, 0.0);
26175         assertEquals(calibrator.getBiasY(), biasY, 0.0);
26176         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
26177         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
26178         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26179         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26180         final Acceleration bx2 = new Acceleration(0.0,
26181                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26182         calibrator.getBiasXAsAcceleration(bx2);
26183         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26184         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26185         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
26186         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26187         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26188         final Acceleration by2 = new Acceleration(0.0,
26189                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26190         calibrator.getBiasYAsAcceleration(by2);
26191         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26192         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26193         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
26194         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26195         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26196         final Acceleration bz2 = new Acceleration(0.0,
26197                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26198         calibrator.getBiasZAsAcceleration(bz2);
26199         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26200         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26201         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26202         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26203         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26204         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26205         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26206         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26207         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26208         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26209         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26210         final double[] bias1 = calibrator.getBias();
26211         assertArrayEquals(bias1, bias, 0.0);
26212         final double[] bias2 = new double[3];
26213         calibrator.getBias(bias2);
26214         assertArrayEquals(bias1, bias2, 0.0);
26215         final Matrix b1 = calibrator.getBiasAsMatrix();
26216         assertEquals(b1, ba);
26217         final Matrix b2 = new Matrix(3, 1);
26218         calibrator.getBiasAsMatrix(b2);
26219         assertEquals(b1, b2);
26220         final Matrix ma1 = calibrator.getInitialMa();
26221         assertEquals(ma1, new Matrix(3, 3));
26222         final Matrix ma2 = new Matrix(3, 3);
26223         calibrator.getInitialMa(ma2);
26224         assertEquals(ma1, ma2);
26225         assertSame(calibrator.getMeasurements(), measurements);
26226         assertTrue(calibrator.isCommonAxisUsed());
26227         assertNull(calibrator.getListener());
26228         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
26229         assertFalse(calibrator.isReady());
26230         assertFalse(calibrator.isRunning());
26231         assertNull(calibrator.getEstimatedMa());
26232         assertNull(calibrator.getEstimatedSx());
26233         assertNull(calibrator.getEstimatedSy());
26234         assertNull(calibrator.getEstimatedSz());
26235         assertNull(calibrator.getEstimatedMxy());
26236         assertNull(calibrator.getEstimatedMxz());
26237         assertNull(calibrator.getEstimatedMyx());
26238         assertNull(calibrator.getEstimatedMyz());
26239         assertNull(calibrator.getEstimatedMzx());
26240         assertNull(calibrator.getEstimatedMzy());
26241         assertNull(calibrator.getEstimatedCovariance());
26242         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26243         assertNotNull(calibrator.getGroundTruthGravityNorm());
26244         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26245         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26246         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26247                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26248         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26249         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26250         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26251 
26252         // Force IllegalArgumentException
26253         final Acceleration invalidGravityNorm = new Acceleration(
26254                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26255 
26256         calibrator = null;
26257         try {
26258             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26259                     invalidGravityNorm, measurements,
26260                     true, bias);
26261             fail("IllegalArgumentException expected but not thrown");
26262         } catch (final IllegalArgumentException ignore) {
26263         }
26264         try {
26265             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26266                     gravityNorm, measurements,
26267                     true, new double[1]);
26268             fail("IllegalArgumentException expected but not thrown");
26269         } catch (final IllegalArgumentException ignore) {
26270         }
26271         assertNull(calibrator);
26272     }
26273 
26274     @Test
26275     public void testConstructor218() throws WrongSizeException {
26276         final Collection<StandardDeviationBodyKinematics> measurements =
26277                 Collections.emptyList();
26278 
26279         final Matrix ba = generateBa();
26280         final double[] bias = ba.getBuffer();
26281         final double biasX = ba.getElementAtIndex(0);
26282         final double biasY = ba.getElementAtIndex(1);
26283         final double biasZ = ba.getElementAtIndex(2);
26284 
26285         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26286         final double latitude = Math.toRadians(
26287                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26288         final double longitude = Math.toRadians(
26289                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26290         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26291         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26292         final NEDVelocity nedVelocity = new NEDVelocity();
26293         final ECEFPosition ecefPosition = new ECEFPosition();
26294         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26295         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26296                 ecefPosition, ecefVelocity);
26297         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26298                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26299         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26300 
26301         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
26302                 new KnownBiasAndGravityNormAccelerometerCalibrator(
26303                         gravityNorm, measurements,
26304                         true, bias, this);
26305 
26306         // check default values
26307         assertEquals(calibrator.getBiasX(), biasX, 0.0);
26308         assertEquals(calibrator.getBiasY(), biasY, 0.0);
26309         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
26310         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
26311         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26312         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26313         final Acceleration bx2 = new Acceleration(0.0,
26314                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26315         calibrator.getBiasXAsAcceleration(bx2);
26316         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26317         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26318         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
26319         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26320         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26321         final Acceleration by2 = new Acceleration(0.0,
26322                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26323         calibrator.getBiasYAsAcceleration(by2);
26324         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26325         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26326         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
26327         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26328         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26329         final Acceleration bz2 = new Acceleration(0.0,
26330                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26331         calibrator.getBiasZAsAcceleration(bz2);
26332         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26333         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26334         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26335         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26336         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26337         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26338         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26339         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26340         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26341         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26342         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26343         final double[] bias1 = calibrator.getBias();
26344         assertArrayEquals(bias1, bias, 0.0);
26345         final double[] bias2 = new double[3];
26346         calibrator.getBias(bias2);
26347         assertArrayEquals(bias1, bias2, 0.0);
26348         final Matrix b1 = calibrator.getBiasAsMatrix();
26349         assertEquals(b1, ba);
26350         final Matrix b2 = new Matrix(3, 1);
26351         calibrator.getBiasAsMatrix(b2);
26352         assertEquals(b1, b2);
26353         final Matrix ma1 = calibrator.getInitialMa();
26354         assertEquals(ma1, new Matrix(3, 3));
26355         final Matrix ma2 = new Matrix(3, 3);
26356         calibrator.getInitialMa(ma2);
26357         assertEquals(ma1, ma2);
26358         assertSame(calibrator.getMeasurements(), measurements);
26359         assertTrue(calibrator.isCommonAxisUsed());
26360         assertSame(calibrator.getListener(), this);
26361         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
26362         assertFalse(calibrator.isReady());
26363         assertFalse(calibrator.isRunning());
26364         assertNull(calibrator.getEstimatedMa());
26365         assertNull(calibrator.getEstimatedSx());
26366         assertNull(calibrator.getEstimatedSy());
26367         assertNull(calibrator.getEstimatedSz());
26368         assertNull(calibrator.getEstimatedMxy());
26369         assertNull(calibrator.getEstimatedMxz());
26370         assertNull(calibrator.getEstimatedMyx());
26371         assertNull(calibrator.getEstimatedMyz());
26372         assertNull(calibrator.getEstimatedMzx());
26373         assertNull(calibrator.getEstimatedMzy());
26374         assertNull(calibrator.getEstimatedCovariance());
26375         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26376         assertNotNull(calibrator.getGroundTruthGravityNorm());
26377         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26378         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26379         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26380                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26381         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26382         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26383         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26384 
26385         // Force IllegalArgumentException
26386         final Acceleration invalidGravityNorm = new Acceleration(
26387                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26388 
26389         calibrator = null;
26390         try {
26391             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26392                     invalidGravityNorm, measurements,
26393                     true, bias, this);
26394             fail("IllegalArgumentException expected but not thrown");
26395         } catch (final IllegalArgumentException ignore) {
26396         }
26397         try {
26398             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26399                     gravityNorm, measurements,
26400                     true, new double[1], this);
26401             fail("IllegalArgumentException expected but not thrown");
26402         } catch (final IllegalArgumentException ignore) {
26403         }
26404         assertNull(calibrator);
26405     }
26406 
26407     @Test
26408     public void testConstructor219() throws WrongSizeException {
26409         final Matrix ba = generateBa();
26410         final double[] bias = ba.getBuffer();
26411         final double biasX = ba.getElementAtIndex(0);
26412         final double biasY = ba.getElementAtIndex(1);
26413         final double biasZ = ba.getElementAtIndex(2);
26414 
26415         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26416         final double latitude = Math.toRadians(
26417                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26418         final double longitude = Math.toRadians(
26419                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26420         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26421         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26422         final NEDVelocity nedVelocity = new NEDVelocity();
26423         final ECEFPosition ecefPosition = new ECEFPosition();
26424         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26425         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26426                 ecefPosition, ecefVelocity);
26427         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26428                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26429         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26430 
26431         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
26432                 new KnownBiasAndGravityNormAccelerometerCalibrator(
26433                         gravityNorm, ba);
26434 
26435         // check default values
26436         assertEquals(calibrator.getBiasX(), biasX, 0.0);
26437         assertEquals(calibrator.getBiasY(), biasY, 0.0);
26438         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
26439         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
26440         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26441         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26442         final Acceleration bx2 = new Acceleration(0.0,
26443                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26444         calibrator.getBiasXAsAcceleration(bx2);
26445         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26446         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26447         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
26448         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26449         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26450         final Acceleration by2 = new Acceleration(0.0,
26451                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26452         calibrator.getBiasYAsAcceleration(by2);
26453         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26454         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26455         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
26456         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26457         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26458         final Acceleration bz2 = new Acceleration(0.0,
26459                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26460         calibrator.getBiasZAsAcceleration(bz2);
26461         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26462         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26463         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26464         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26465         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26466         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26467         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26468         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26469         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26470         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26471         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26472         final double[] bias1 = calibrator.getBias();
26473         assertArrayEquals(bias1, bias, 0.0);
26474         final double[] bias2 = new double[3];
26475         calibrator.getBias(bias2);
26476         assertArrayEquals(bias1, bias2, 0.0);
26477         final Matrix b1 = calibrator.getBiasAsMatrix();
26478         assertEquals(b1, ba);
26479         final Matrix b2 = new Matrix(3, 1);
26480         calibrator.getBiasAsMatrix(b2);
26481         assertEquals(b1, b2);
26482         final Matrix ma1 = calibrator.getInitialMa();
26483         assertEquals(ma1, new Matrix(3, 3));
26484         final Matrix ma2 = new Matrix(3, 3);
26485         calibrator.getInitialMa(ma2);
26486         assertEquals(ma1, ma2);
26487         assertNull(calibrator.getMeasurements());
26488         assertFalse(calibrator.isCommonAxisUsed());
26489         assertNull(calibrator.getListener());
26490         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26491         assertFalse(calibrator.isReady());
26492         assertFalse(calibrator.isRunning());
26493         assertNull(calibrator.getEstimatedMa());
26494         assertNull(calibrator.getEstimatedSx());
26495         assertNull(calibrator.getEstimatedSy());
26496         assertNull(calibrator.getEstimatedSz());
26497         assertNull(calibrator.getEstimatedMxy());
26498         assertNull(calibrator.getEstimatedMxz());
26499         assertNull(calibrator.getEstimatedMyx());
26500         assertNull(calibrator.getEstimatedMyz());
26501         assertNull(calibrator.getEstimatedMzx());
26502         assertNull(calibrator.getEstimatedMzy());
26503         assertNull(calibrator.getEstimatedCovariance());
26504         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26505         assertNotNull(calibrator.getGroundTruthGravityNorm());
26506         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26507         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26508         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26509                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26510         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26511         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26512         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26513 
26514         // Force IllegalArgumentException
26515         final Acceleration invalidGravityNorm = new Acceleration(
26516                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26517 
26518         calibrator = null;
26519         try {
26520             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26521                     invalidGravityNorm, ba);
26522             fail("IllegalArgumentException expected but not thrown");
26523         } catch (final IllegalArgumentException ignore) {
26524         }
26525         try {
26526             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26527                     gravityNorm,
26528                     new Matrix(1, 1));
26529             fail("IllegalArgumentException expected but not thrown");
26530         } catch (final IllegalArgumentException ignore) {
26531         }
26532         try {
26533             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26534                     gravityNorm, new Matrix(1, 3));
26535             fail("IllegalArgumentException expected but not thrown");
26536         } catch (final IllegalArgumentException ignore) {
26537         }
26538         assertNull(calibrator);
26539     }
26540 
26541     @Test
26542     public void testConstructor220() throws WrongSizeException {
26543         final Matrix ba = generateBa();
26544         final double[] bias = ba.getBuffer();
26545         final double biasX = ba.getElementAtIndex(0);
26546         final double biasY = ba.getElementAtIndex(1);
26547         final double biasZ = ba.getElementAtIndex(2);
26548 
26549         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26550         final double latitude = Math.toRadians(
26551                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26552         final double longitude = Math.toRadians(
26553                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26554         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26555         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26556         final NEDVelocity nedVelocity = new NEDVelocity();
26557         final ECEFPosition ecefPosition = new ECEFPosition();
26558         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26559         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26560                 ecefPosition, ecefVelocity);
26561         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26562                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26563         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26564 
26565         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
26566                 new KnownBiasAndGravityNormAccelerometerCalibrator(
26567                         gravityNorm, ba, this);
26568 
26569         // check default values
26570         assertEquals(calibrator.getBiasX(), biasX, 0.0);
26571         assertEquals(calibrator.getBiasY(), biasY, 0.0);
26572         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
26573         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
26574         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26575         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26576         final Acceleration bx2 = new Acceleration(0.0,
26577                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26578         calibrator.getBiasXAsAcceleration(bx2);
26579         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26580         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26581         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
26582         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26583         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26584         final Acceleration by2 = new Acceleration(0.0,
26585                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26586         calibrator.getBiasYAsAcceleration(by2);
26587         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26588         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26589         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
26590         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26591         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26592         final Acceleration bz2 = new Acceleration(0.0,
26593                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26594         calibrator.getBiasZAsAcceleration(bz2);
26595         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26596         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26597         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26598         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26599         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26600         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26601         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26602         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26603         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26604         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26605         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26606         final double[] bias1 = calibrator.getBias();
26607         assertArrayEquals(bias1, bias, 0.0);
26608         final double[] bias2 = new double[3];
26609         calibrator.getBias(bias2);
26610         assertArrayEquals(bias1, bias2, 0.0);
26611         final Matrix b1 = calibrator.getBiasAsMatrix();
26612         assertEquals(b1, ba);
26613         final Matrix b2 = new Matrix(3, 1);
26614         calibrator.getBiasAsMatrix(b2);
26615         assertEquals(b1, b2);
26616         final Matrix ma1 = calibrator.getInitialMa();
26617         assertEquals(ma1, new Matrix(3, 3));
26618         final Matrix ma2 = new Matrix(3, 3);
26619         calibrator.getInitialMa(ma2);
26620         assertEquals(ma1, ma2);
26621         assertNull(calibrator.getMeasurements());
26622         assertFalse(calibrator.isCommonAxisUsed());
26623         assertSame(calibrator.getListener(), this);
26624         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26625         assertFalse(calibrator.isReady());
26626         assertFalse(calibrator.isRunning());
26627         assertNull(calibrator.getEstimatedMa());
26628         assertNull(calibrator.getEstimatedSx());
26629         assertNull(calibrator.getEstimatedSy());
26630         assertNull(calibrator.getEstimatedSz());
26631         assertNull(calibrator.getEstimatedMxy());
26632         assertNull(calibrator.getEstimatedMxz());
26633         assertNull(calibrator.getEstimatedMyx());
26634         assertNull(calibrator.getEstimatedMyz());
26635         assertNull(calibrator.getEstimatedMzx());
26636         assertNull(calibrator.getEstimatedMzy());
26637         assertNull(calibrator.getEstimatedCovariance());
26638         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26639         assertNotNull(calibrator.getGroundTruthGravityNorm());
26640         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26641         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26642         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26643                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26644         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26645         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26646         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26647 
26648         // Force IllegalArgumentException
26649         final Acceleration invalidGravityNorm = new Acceleration(
26650                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26651 
26652         calibrator = null;
26653         try {
26654             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26655                     invalidGravityNorm, ba);
26656             fail("IllegalArgumentException expected but not thrown");
26657         } catch (final IllegalArgumentException ignore) {
26658         }
26659         try {
26660             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26661                     gravityNorm, new Matrix(1, 1),
26662                     this);
26663             fail("IllegalArgumentException expected but not thrown");
26664         } catch (final IllegalArgumentException ignore) {
26665         }
26666         try {
26667             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26668                     gravityNorm, new Matrix(1, 3),
26669                     this);
26670             fail("IllegalArgumentException expected but not thrown");
26671         } catch (final IllegalArgumentException ignore) {
26672         }
26673         assertNull(calibrator);
26674     }
26675 
26676     @Test
26677     public void testConstructor221() throws WrongSizeException {
26678         final Collection<StandardDeviationBodyKinematics> measurements =
26679                 Collections.emptyList();
26680 
26681         final Matrix ba = generateBa();
26682         final double[] bias = ba.getBuffer();
26683         final double biasX = ba.getElementAtIndex(0);
26684         final double biasY = ba.getElementAtIndex(1);
26685         final double biasZ = ba.getElementAtIndex(2);
26686 
26687         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26688         final double latitude = Math.toRadians(
26689                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26690         final double longitude = Math.toRadians(
26691                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26692         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26693         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26694         final NEDVelocity nedVelocity = new NEDVelocity();
26695         final ECEFPosition ecefPosition = new ECEFPosition();
26696         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26697         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26698                 ecefPosition, ecefVelocity);
26699         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26700                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26701         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26702 
26703         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
26704                 new KnownBiasAndGravityNormAccelerometerCalibrator(
26705                         gravityNorm, measurements, ba);
26706 
26707         // check default values
26708         assertEquals(calibrator.getBiasX(), biasX, 0.0);
26709         assertEquals(calibrator.getBiasY(), biasY, 0.0);
26710         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
26711         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
26712         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26713         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26714         final Acceleration bx2 = new Acceleration(0.0,
26715                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26716         calibrator.getBiasXAsAcceleration(bx2);
26717         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26718         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26719         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
26720         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26721         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26722         final Acceleration by2 = new Acceleration(0.0,
26723                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26724         calibrator.getBiasYAsAcceleration(by2);
26725         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26726         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26727         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
26728         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26729         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26730         final Acceleration bz2 = new Acceleration(0.0,
26731                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26732         calibrator.getBiasZAsAcceleration(bz2);
26733         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26734         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26735         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26736         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26737         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26738         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26739         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26740         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26741         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26742         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26743         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26744         final double[] bias1 = calibrator.getBias();
26745         assertArrayEquals(bias1, bias, 0.0);
26746         final double[] bias2 = new double[3];
26747         calibrator.getBias(bias2);
26748         assertArrayEquals(bias1, bias2, 0.0);
26749         final Matrix b1 = calibrator.getBiasAsMatrix();
26750         assertEquals(b1, ba);
26751         final Matrix b2 = new Matrix(3, 1);
26752         calibrator.getBiasAsMatrix(b2);
26753         assertEquals(b1, b2);
26754         final Matrix ma1 = calibrator.getInitialMa();
26755         assertEquals(ma1, new Matrix(3, 3));
26756         final Matrix ma2 = new Matrix(3, 3);
26757         calibrator.getInitialMa(ma2);
26758         assertEquals(ma1, ma2);
26759         assertSame(calibrator.getMeasurements(), measurements);
26760         assertFalse(calibrator.isCommonAxisUsed());
26761         assertNull(calibrator.getListener());
26762         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26763         assertFalse(calibrator.isReady());
26764         assertFalse(calibrator.isRunning());
26765         assertNull(calibrator.getEstimatedMa());
26766         assertNull(calibrator.getEstimatedSx());
26767         assertNull(calibrator.getEstimatedSy());
26768         assertNull(calibrator.getEstimatedSz());
26769         assertNull(calibrator.getEstimatedMxy());
26770         assertNull(calibrator.getEstimatedMxz());
26771         assertNull(calibrator.getEstimatedMyx());
26772         assertNull(calibrator.getEstimatedMyz());
26773         assertNull(calibrator.getEstimatedMzx());
26774         assertNull(calibrator.getEstimatedMzy());
26775         assertNull(calibrator.getEstimatedCovariance());
26776         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26777         assertNotNull(calibrator.getGroundTruthGravityNorm());
26778         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26779         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26780         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26781                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26782         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26783         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26784         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26785 
26786         // Force IllegalArgumentException
26787         final Acceleration invalidGravityNorm = new Acceleration(
26788                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26789 
26790         calibrator = null;
26791         try {
26792             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26793                     invalidGravityNorm, measurements, ba);
26794             fail("IllegalArgumentException expected but not thrown");
26795         } catch (final IllegalArgumentException ignore) {
26796         }
26797         try {
26798             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26799                     gravityNorm, measurements,
26800                     new Matrix(1, 1));
26801             fail("IllegalArgumentException expected but not thrown");
26802         } catch (final IllegalArgumentException ignore) {
26803         }
26804         try {
26805             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26806                     gravityNorm, measurements,
26807                     new Matrix(1, 3));
26808             fail("IllegalArgumentException expected but not thrown");
26809         } catch (final IllegalArgumentException ignore) {
26810         }
26811         assertNull(calibrator);
26812     }
26813 
26814     @Test
26815     public void testConstructor222() throws WrongSizeException {
26816         final Collection<StandardDeviationBodyKinematics> measurements =
26817                 Collections.emptyList();
26818 
26819         final Matrix ba = generateBa();
26820         final double[] bias = ba.getBuffer();
26821         final double biasX = ba.getElementAtIndex(0);
26822         final double biasY = ba.getElementAtIndex(1);
26823         final double biasZ = ba.getElementAtIndex(2);
26824 
26825         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26826         final double latitude = Math.toRadians(
26827                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26828         final double longitude = Math.toRadians(
26829                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26830         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26831         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26832         final NEDVelocity nedVelocity = new NEDVelocity();
26833         final ECEFPosition ecefPosition = new ECEFPosition();
26834         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26835         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26836                 ecefPosition, ecefVelocity);
26837         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26838                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26839         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26840 
26841         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
26842                 new KnownBiasAndGravityNormAccelerometerCalibrator(
26843                         gravityNorm, measurements,
26844                         ba, this);
26845 
26846         // check default values
26847         assertEquals(calibrator.getBiasX(), biasX, 0.0);
26848         assertEquals(calibrator.getBiasY(), biasY, 0.0);
26849         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
26850         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
26851         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26852         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26853         final Acceleration bx2 = new Acceleration(0.0,
26854                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26855         calibrator.getBiasXAsAcceleration(bx2);
26856         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26857         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26858         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
26859         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26860         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26861         final Acceleration by2 = new Acceleration(0.0,
26862                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26863         calibrator.getBiasYAsAcceleration(by2);
26864         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26865         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26866         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
26867         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26868         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26869         final Acceleration bz2 = new Acceleration(0.0,
26870                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26871         calibrator.getBiasZAsAcceleration(bz2);
26872         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26873         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26874         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26875         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26876         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26877         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26878         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26879         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26880         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26881         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26882         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26883         final double[] bias1 = calibrator.getBias();
26884         assertArrayEquals(bias1, bias, 0.0);
26885         final double[] bias2 = new double[3];
26886         calibrator.getBias(bias2);
26887         assertArrayEquals(bias1, bias2, 0.0);
26888         final Matrix b1 = calibrator.getBiasAsMatrix();
26889         assertEquals(b1, ba);
26890         final Matrix b2 = new Matrix(3, 1);
26891         calibrator.getBiasAsMatrix(b2);
26892         assertEquals(b1, b2);
26893         final Matrix ma1 = calibrator.getInitialMa();
26894         assertEquals(ma1, new Matrix(3, 3));
26895         final Matrix ma2 = new Matrix(3, 3);
26896         calibrator.getInitialMa(ma2);
26897         assertEquals(ma1, ma2);
26898         assertSame(calibrator.getMeasurements(), measurements);
26899         assertFalse(calibrator.isCommonAxisUsed());
26900         assertSame(calibrator.getListener(), this);
26901         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26902         assertFalse(calibrator.isReady());
26903         assertFalse(calibrator.isRunning());
26904         assertNull(calibrator.getEstimatedMa());
26905         assertNull(calibrator.getEstimatedSx());
26906         assertNull(calibrator.getEstimatedSy());
26907         assertNull(calibrator.getEstimatedSz());
26908         assertNull(calibrator.getEstimatedMxy());
26909         assertNull(calibrator.getEstimatedMxz());
26910         assertNull(calibrator.getEstimatedMyx());
26911         assertNull(calibrator.getEstimatedMyz());
26912         assertNull(calibrator.getEstimatedMzx());
26913         assertNull(calibrator.getEstimatedMzy());
26914         assertNull(calibrator.getEstimatedCovariance());
26915         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26916         assertNotNull(calibrator.getGroundTruthGravityNorm());
26917         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26918         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26919         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26920                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26921         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26922         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26923         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26924 
26925         // Force IllegalArgumentException
26926         final Acceleration invalidGravityNorm = new Acceleration(
26927                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26928 
26929         calibrator = null;
26930         try {
26931             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26932                     invalidGravityNorm, measurements,
26933                     ba, this);
26934             fail("IllegalArgumentException expected but not thrown");
26935         } catch (final IllegalArgumentException ignore) {
26936         }
26937         try {
26938             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26939                     gravityNorm, measurements,
26940                     new Matrix(1, 1), this);
26941             fail("IllegalArgumentException expected but not thrown");
26942         } catch (final IllegalArgumentException ignore) {
26943         }
26944         try {
26945             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
26946                     gravityNorm, measurements,
26947                     new Matrix(1, 3), this);
26948             fail("IllegalArgumentException expected but not thrown");
26949         } catch (final IllegalArgumentException ignore) {
26950         }
26951         assertNull(calibrator);
26952     }
26953 
26954     @Test
26955     public void testConstructor223() throws WrongSizeException {
26956         final Matrix ba = generateBa();
26957         final double[] bias = ba.getBuffer();
26958         final double biasX = ba.getElementAtIndex(0);
26959         final double biasY = ba.getElementAtIndex(1);
26960         final double biasZ = ba.getElementAtIndex(2);
26961 
26962         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26963         final double latitude = Math.toRadians(
26964                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26965         final double longitude = Math.toRadians(
26966                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26967         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26968         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26969         final NEDVelocity nedVelocity = new NEDVelocity();
26970         final ECEFPosition ecefPosition = new ECEFPosition();
26971         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26972         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26973                 ecefPosition, ecefVelocity);
26974         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26975                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26976         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26977 
26978         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
26979                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
26980                         true, ba);
26981 
26982         // check default values
26983         assertEquals(calibrator.getBiasX(), biasX, 0.0);
26984         assertEquals(calibrator.getBiasY(), biasY, 0.0);
26985         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
26986         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
26987         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26988         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26989         final Acceleration bx2 = new Acceleration(0.0,
26990                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26991         calibrator.getBiasXAsAcceleration(bx2);
26992         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26993         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26994         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
26995         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26996         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26997         final Acceleration by2 = new Acceleration(0.0,
26998                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26999         calibrator.getBiasYAsAcceleration(by2);
27000         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27001         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27002         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
27003         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27004         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27005         final Acceleration bz2 = new Acceleration(0.0,
27006                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27007         calibrator.getBiasZAsAcceleration(bz2);
27008         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27009         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27010         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27011         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27012         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27013         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27014         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27015         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27016         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27017         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27018         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27019         final double[] bias1 = calibrator.getBias();
27020         assertArrayEquals(bias1, bias, 0.0);
27021         final double[] bias2 = new double[3];
27022         calibrator.getBias(bias2);
27023         assertArrayEquals(bias1, bias2, 0.0);
27024         final Matrix b1 = calibrator.getBiasAsMatrix();
27025         assertEquals(b1, ba);
27026         final Matrix b2 = new Matrix(3, 1);
27027         calibrator.getBiasAsMatrix(b2);
27028         assertEquals(b1, b2);
27029         final Matrix ma1 = calibrator.getInitialMa();
27030         assertEquals(ma1, new Matrix(3, 3));
27031         final Matrix ma2 = new Matrix(3, 3);
27032         calibrator.getInitialMa(ma2);
27033         assertEquals(ma1, ma2);
27034         assertNull(calibrator.getMeasurements());
27035         assertTrue(calibrator.isCommonAxisUsed());
27036         assertNull(calibrator.getListener());
27037         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
27038         assertFalse(calibrator.isReady());
27039         assertFalse(calibrator.isRunning());
27040         assertNull(calibrator.getEstimatedMa());
27041         assertNull(calibrator.getEstimatedSx());
27042         assertNull(calibrator.getEstimatedSy());
27043         assertNull(calibrator.getEstimatedSz());
27044         assertNull(calibrator.getEstimatedMxy());
27045         assertNull(calibrator.getEstimatedMxz());
27046         assertNull(calibrator.getEstimatedMyx());
27047         assertNull(calibrator.getEstimatedMyz());
27048         assertNull(calibrator.getEstimatedMzx());
27049         assertNull(calibrator.getEstimatedMzy());
27050         assertNull(calibrator.getEstimatedCovariance());
27051         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27052         assertNotNull(calibrator.getGroundTruthGravityNorm());
27053         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27054         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27055         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27056                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27057         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27058         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27059         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27060 
27061         // Force IllegalArgumentException
27062         final Acceleration invalidGravityNorm = new Acceleration(
27063                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27064 
27065         calibrator = null;
27066         try {
27067             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27068                     invalidGravityNorm, true, ba);
27069             fail("IllegalArgumentException expected but not thrown");
27070         } catch (final IllegalArgumentException ignore) {
27071         }
27072         try {
27073             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27074                     gravityNorm, true,
27075                     new Matrix(1, 1));
27076             fail("IllegalArgumentException expected but not thrown");
27077         } catch (final IllegalArgumentException ignore) {
27078         }
27079         try {
27080             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27081                     gravityNorm, true,
27082                     new Matrix(1, 3));
27083             fail("IllegalArgumentException expected but not thrown");
27084         } catch (final IllegalArgumentException ignore) {
27085         }
27086         assertNull(calibrator);
27087     }
27088 
27089     @Test
27090     public void testConstructor224() throws WrongSizeException {
27091         final Matrix ba = generateBa();
27092         final double[] bias = ba.getBuffer();
27093         final double biasX = ba.getElementAtIndex(0);
27094         final double biasY = ba.getElementAtIndex(1);
27095         final double biasZ = ba.getElementAtIndex(2);
27096 
27097         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27098         final double latitude = Math.toRadians(
27099                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27100         final double longitude = Math.toRadians(
27101                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27102         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27103         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27104         final NEDVelocity nedVelocity = new NEDVelocity();
27105         final ECEFPosition ecefPosition = new ECEFPosition();
27106         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27107         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27108                 ecefPosition, ecefVelocity);
27109         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27110                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27111         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27112 
27113         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
27114                 new KnownBiasAndGravityNormAccelerometerCalibrator(
27115                         gravityNorm, true, ba, this);
27116 
27117         // check default values
27118         assertEquals(calibrator.getBiasX(), biasX, 0.0);
27119         assertEquals(calibrator.getBiasY(), biasY, 0.0);
27120         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
27121         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
27122         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27123         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27124         final Acceleration bx2 = new Acceleration(0.0,
27125                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27126         calibrator.getBiasXAsAcceleration(bx2);
27127         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27128         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27129         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
27130         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27131         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27132         final Acceleration by2 = new Acceleration(0.0,
27133                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27134         calibrator.getBiasYAsAcceleration(by2);
27135         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27136         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27137         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
27138         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27139         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27140         final Acceleration bz2 = new Acceleration(0.0,
27141                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27142         calibrator.getBiasZAsAcceleration(bz2);
27143         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27144         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27145         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27146         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27147         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27148         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27149         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27150         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27151         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27152         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27153         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27154         final double[] bias1 = calibrator.getBias();
27155         assertArrayEquals(bias1, bias, 0.0);
27156         final double[] bias2 = new double[3];
27157         calibrator.getBias(bias2);
27158         assertArrayEquals(bias1, bias2, 0.0);
27159         final Matrix b1 = calibrator.getBiasAsMatrix();
27160         assertEquals(b1, ba);
27161         final Matrix b2 = new Matrix(3, 1);
27162         calibrator.getBiasAsMatrix(b2);
27163         assertEquals(b1, b2);
27164         final Matrix ma1 = calibrator.getInitialMa();
27165         assertEquals(ma1, new Matrix(3, 3));
27166         final Matrix ma2 = new Matrix(3, 3);
27167         calibrator.getInitialMa(ma2);
27168         assertEquals(ma1, ma2);
27169         assertNull(calibrator.getMeasurements());
27170         assertTrue(calibrator.isCommonAxisUsed());
27171         assertSame(calibrator.getListener(), this);
27172         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
27173         assertFalse(calibrator.isReady());
27174         assertFalse(calibrator.isRunning());
27175         assertNull(calibrator.getEstimatedMa());
27176         assertNull(calibrator.getEstimatedSx());
27177         assertNull(calibrator.getEstimatedSy());
27178         assertNull(calibrator.getEstimatedSz());
27179         assertNull(calibrator.getEstimatedMxy());
27180         assertNull(calibrator.getEstimatedMxz());
27181         assertNull(calibrator.getEstimatedMyx());
27182         assertNull(calibrator.getEstimatedMyz());
27183         assertNull(calibrator.getEstimatedMzx());
27184         assertNull(calibrator.getEstimatedMzy());
27185         assertNull(calibrator.getEstimatedCovariance());
27186         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27187         assertNotNull(calibrator.getGroundTruthGravityNorm());
27188         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27189         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27190         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27191                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27192         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27193         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27194         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27195 
27196         // Force IllegalArgumentException
27197         final Acceleration invalidGravityNorm = new Acceleration(
27198                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27199 
27200         calibrator = null;
27201         try {
27202             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27203                     invalidGravityNorm, true, ba, this);
27204             fail("IllegalArgumentException expected but not thrown");
27205         } catch (final IllegalArgumentException ignore) {
27206         }
27207         try {
27208             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27209                     gravityNorm, true, new Matrix(1, 1),
27210                     this);
27211             fail("IllegalArgumentException expected but not thrown");
27212         } catch (final IllegalArgumentException ignore) {
27213         }
27214         try {
27215             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27216                     gravityNorm, true, new Matrix(1, 3),
27217                     this);
27218             fail("IllegalArgumentException expected but not thrown");
27219         } catch (final IllegalArgumentException ignore) {
27220         }
27221         assertNull(calibrator);
27222     }
27223 
27224     @Test
27225     public void testConstructor225() throws WrongSizeException {
27226         final Collection<StandardDeviationBodyKinematics> measurements =
27227                 Collections.emptyList();
27228 
27229         final Matrix ba = generateBa();
27230         final double[] bias = ba.getBuffer();
27231         final double biasX = ba.getElementAtIndex(0);
27232         final double biasY = ba.getElementAtIndex(1);
27233         final double biasZ = ba.getElementAtIndex(2);
27234 
27235         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27236         final double latitude = Math.toRadians(
27237                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27238         final double longitude = Math.toRadians(
27239                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27240         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27241         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27242         final NEDVelocity nedVelocity = new NEDVelocity();
27243         final ECEFPosition ecefPosition = new ECEFPosition();
27244         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27245         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27246                 ecefPosition, ecefVelocity);
27247         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27248                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27249         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27250 
27251         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
27252                 new KnownBiasAndGravityNormAccelerometerCalibrator(
27253                         gravityNorm, measurements,
27254                         true, ba);
27255 
27256         // check default values
27257         assertEquals(calibrator.getBiasX(), biasX, 0.0);
27258         assertEquals(calibrator.getBiasY(), biasY, 0.0);
27259         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
27260         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
27261         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27262         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27263         final Acceleration bx2 = new Acceleration(0.0,
27264                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27265         calibrator.getBiasXAsAcceleration(bx2);
27266         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27267         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27268         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
27269         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27270         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27271         final Acceleration by2 = new Acceleration(0.0,
27272                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27273         calibrator.getBiasYAsAcceleration(by2);
27274         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27275         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27276         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
27277         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27278         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27279         final Acceleration bz2 = new Acceleration(0.0,
27280                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27281         calibrator.getBiasZAsAcceleration(bz2);
27282         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27283         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27284         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27285         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27286         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27287         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27288         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27289         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27290         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27291         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27292         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27293         final double[] bias1 = calibrator.getBias();
27294         assertArrayEquals(bias1, bias, 0.0);
27295         final double[] bias2 = new double[3];
27296         calibrator.getBias(bias2);
27297         assertArrayEquals(bias1, bias2, 0.0);
27298         final Matrix b1 = calibrator.getBiasAsMatrix();
27299         assertEquals(b1, ba);
27300         final Matrix b2 = new Matrix(3, 1);
27301         calibrator.getBiasAsMatrix(b2);
27302         assertEquals(b1, b2);
27303         final Matrix ma1 = calibrator.getInitialMa();
27304         assertEquals(ma1, new Matrix(3, 3));
27305         final Matrix ma2 = new Matrix(3, 3);
27306         calibrator.getInitialMa(ma2);
27307         assertEquals(ma1, ma2);
27308         assertSame(calibrator.getMeasurements(), measurements);
27309         assertTrue(calibrator.isCommonAxisUsed());
27310         assertNull(calibrator.getListener());
27311         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
27312         assertFalse(calibrator.isReady());
27313         assertFalse(calibrator.isRunning());
27314         assertNull(calibrator.getEstimatedMa());
27315         assertNull(calibrator.getEstimatedSx());
27316         assertNull(calibrator.getEstimatedSy());
27317         assertNull(calibrator.getEstimatedSz());
27318         assertNull(calibrator.getEstimatedMxy());
27319         assertNull(calibrator.getEstimatedMxz());
27320         assertNull(calibrator.getEstimatedMyx());
27321         assertNull(calibrator.getEstimatedMyz());
27322         assertNull(calibrator.getEstimatedMzx());
27323         assertNull(calibrator.getEstimatedMzy());
27324         assertNull(calibrator.getEstimatedCovariance());
27325         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27326         assertNotNull(calibrator.getGroundTruthGravityNorm());
27327         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27328         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27329         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27330                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27331         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27332         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27333         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27334 
27335         // Force IllegalArgumentException
27336         final Acceleration invalidGravityNorm = new Acceleration(
27337                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27338 
27339         calibrator = null;
27340         try {
27341             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27342                     invalidGravityNorm, measurements,
27343                     true, ba);
27344             fail("IllegalArgumentException expected but not thrown");
27345         } catch (final IllegalArgumentException ignore) {
27346         }
27347         try {
27348             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27349                     gravityNorm, measurements, true,
27350                     new Matrix(1, 1));
27351             fail("IllegalArgumentException expected but not thrown");
27352         } catch (final IllegalArgumentException ignore) {
27353         }
27354         try {
27355             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27356                     gravityNorm, measurements, true,
27357                     new Matrix(1, 3));
27358             fail("IllegalArgumentException expected but not thrown");
27359         } catch (final IllegalArgumentException ignore) {
27360         }
27361         assertNull(calibrator);
27362     }
27363 
27364     @Test
27365     public void testConstructor226() throws WrongSizeException {
27366         final Collection<StandardDeviationBodyKinematics> measurements =
27367                 Collections.emptyList();
27368 
27369         final Matrix ba = generateBa();
27370         final double[] bias = ba.getBuffer();
27371         final double biasX = ba.getElementAtIndex(0);
27372         final double biasY = ba.getElementAtIndex(1);
27373         final double biasZ = ba.getElementAtIndex(2);
27374 
27375         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27376         final double latitude = Math.toRadians(
27377                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27378         final double longitude = Math.toRadians(
27379                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27380         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27381         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27382         final NEDVelocity nedVelocity = new NEDVelocity();
27383         final ECEFPosition ecefPosition = new ECEFPosition();
27384         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27385         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27386                 ecefPosition, ecefVelocity);
27387         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27388                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27389         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27390 
27391         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
27392                 new KnownBiasAndGravityNormAccelerometerCalibrator(
27393                         gravityNorm, measurements,
27394                         true, ba, this);
27395 
27396         // check default values
27397         assertEquals(calibrator.getBiasX(), biasX, 0.0);
27398         assertEquals(calibrator.getBiasY(), biasY, 0.0);
27399         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
27400         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
27401         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27402         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27403         final Acceleration bx2 = new Acceleration(0.0,
27404                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27405         calibrator.getBiasXAsAcceleration(bx2);
27406         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27407         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27408         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
27409         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27410         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27411         final Acceleration by2 = new Acceleration(0.0,
27412                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27413         calibrator.getBiasYAsAcceleration(by2);
27414         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27415         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27416         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
27417         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27418         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27419         final Acceleration bz2 = new Acceleration(0.0,
27420                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27421         calibrator.getBiasZAsAcceleration(bz2);
27422         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27423         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27424         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27425         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27426         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27427         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27428         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27429         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27430         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27431         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27432         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27433         final double[] bias1 = calibrator.getBias();
27434         assertArrayEquals(bias1, bias, 0.0);
27435         final double[] bias2 = new double[3];
27436         calibrator.getBias(bias2);
27437         assertArrayEquals(bias1, bias2, 0.0);
27438         final Matrix b1 = calibrator.getBiasAsMatrix();
27439         assertEquals(b1, ba);
27440         final Matrix b2 = new Matrix(3, 1);
27441         calibrator.getBiasAsMatrix(b2);
27442         assertEquals(b1, b2);
27443         final Matrix ma1 = calibrator.getInitialMa();
27444         assertEquals(ma1, new Matrix(3, 3));
27445         final Matrix ma2 = new Matrix(3, 3);
27446         calibrator.getInitialMa(ma2);
27447         assertEquals(ma1, ma2);
27448         assertSame(calibrator.getMeasurements(), measurements);
27449         assertTrue(calibrator.isCommonAxisUsed());
27450         assertSame(calibrator.getListener(), this);
27451         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
27452         assertFalse(calibrator.isReady());
27453         assertFalse(calibrator.isRunning());
27454         assertNull(calibrator.getEstimatedMa());
27455         assertNull(calibrator.getEstimatedSx());
27456         assertNull(calibrator.getEstimatedSy());
27457         assertNull(calibrator.getEstimatedSz());
27458         assertNull(calibrator.getEstimatedMxy());
27459         assertNull(calibrator.getEstimatedMxz());
27460         assertNull(calibrator.getEstimatedMyx());
27461         assertNull(calibrator.getEstimatedMyz());
27462         assertNull(calibrator.getEstimatedMzx());
27463         assertNull(calibrator.getEstimatedMzy());
27464         assertNull(calibrator.getEstimatedCovariance());
27465         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27466         assertNotNull(calibrator.getGroundTruthGravityNorm());
27467         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27468         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27469         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27470                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27471         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27472         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27473         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27474 
27475         // Force IllegalArgumentException
27476         final Acceleration invalidGravityNorm = new Acceleration(
27477                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27478 
27479         calibrator = null;
27480         try {
27481             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27482                     invalidGravityNorm, measurements,
27483                     true, ba, this);
27484             fail("IllegalArgumentException expected but not thrown");
27485         } catch (final IllegalArgumentException ignore) {
27486         }
27487         try {
27488             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27489                     gravityNorm, measurements, true,
27490                     new Matrix(1, 1), this);
27491             fail("IllegalArgumentException expected but not thrown");
27492         } catch (final IllegalArgumentException ignore) {
27493         }
27494         try {
27495             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27496                     gravityNorm, measurements, true,
27497                     new Matrix(1, 3), this);
27498             fail("IllegalArgumentException expected but not thrown");
27499         } catch (final IllegalArgumentException ignore) {
27500         }
27501         assertNull(calibrator);
27502     }
27503 
27504     @Test
27505     public void testConstructor227() throws WrongSizeException {
27506         final Matrix ba = generateBa();
27507         final double[] bias = ba.getBuffer();
27508         final double biasX = ba.getElementAtIndex(0);
27509         final double biasY = ba.getElementAtIndex(1);
27510         final double biasZ = ba.getElementAtIndex(2);
27511 
27512         final Matrix ma = generateMaCommonAxis();
27513         final double sx = ma.getElementAt(0, 0);
27514         final double sy = ma.getElementAt(1, 1);
27515         final double sz = ma.getElementAt(2, 2);
27516         final double mxy = ma.getElementAt(0, 1);
27517         final double mxz = ma.getElementAt(0, 2);
27518         final double myx = ma.getElementAt(1, 0);
27519         final double myz = ma.getElementAt(1, 2);
27520         final double mzx = ma.getElementAt(2, 0);
27521         final double mzy = ma.getElementAt(2, 1);
27522 
27523         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27524         final double latitude = Math.toRadians(
27525                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27526         final double longitude = Math.toRadians(
27527                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27528         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27529         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27530         final NEDVelocity nedVelocity = new NEDVelocity();
27531         final ECEFPosition ecefPosition = new ECEFPosition();
27532         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27533         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27534                 ecefPosition, ecefVelocity);
27535         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27536                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27537         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27538 
27539         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
27540                 new KnownBiasAndGravityNormAccelerometerCalibrator(
27541                         gravityNorm, ba, ma);
27542 
27543         // check default values
27544         assertEquals(calibrator.getBiasX(), biasX, 0.0);
27545         assertEquals(calibrator.getBiasY(), biasY, 0.0);
27546         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
27547         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
27548         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27549         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27550         final Acceleration bx2 = new Acceleration(0.0,
27551                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27552         calibrator.getBiasXAsAcceleration(bx2);
27553         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27554         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27555         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
27556         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27557         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27558         final Acceleration by2 = new Acceleration(0.0,
27559                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27560         calibrator.getBiasYAsAcceleration(by2);
27561         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27562         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27563         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
27564         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27565         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27566         final Acceleration bz2 = new Acceleration(0.0,
27567                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27568         calibrator.getBiasZAsAcceleration(bz2);
27569         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27570         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27571         assertEquals(calibrator.getInitialSx(), sx, 0.0);
27572         assertEquals(calibrator.getInitialSy(), sy, 0.0);
27573         assertEquals(calibrator.getInitialSz(), sz, 0.0);
27574         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27575         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27576         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27577         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27578         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27579         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27580         final double[] bias1 = calibrator.getBias();
27581         assertArrayEquals(bias1, bias, 0.0);
27582         final double[] bias2 = new double[3];
27583         calibrator.getBias(bias2);
27584         assertArrayEquals(bias1, bias2, 0.0);
27585         final Matrix b1 = calibrator.getBiasAsMatrix();
27586         assertEquals(b1, ba);
27587         final Matrix b2 = new Matrix(3, 1);
27588         calibrator.getBiasAsMatrix(b2);
27589         assertEquals(b1, b2);
27590         final Matrix ma1 = new Matrix(3, 3);
27591         ma1.setSubmatrix(0, 0,
27592                 2, 2,
27593                 new double[]{sx, myx, mzx,
27594                         mxy, sy, mzy,
27595                         mxz, myz, sz});
27596         assertEquals(calibrator.getInitialMa(), ma1);
27597         final Matrix ma2 = new Matrix(3, 3);
27598         calibrator.getInitialMa(ma2);
27599         assertEquals(ma1, ma2);
27600         assertNull(calibrator.getMeasurements());
27601         assertFalse(calibrator.isCommonAxisUsed());
27602         assertNull(calibrator.getListener());
27603         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27604         assertFalse(calibrator.isReady());
27605         assertFalse(calibrator.isRunning());
27606         assertNull(calibrator.getEstimatedMa());
27607         assertNull(calibrator.getEstimatedSx());
27608         assertNull(calibrator.getEstimatedSy());
27609         assertNull(calibrator.getEstimatedSz());
27610         assertNull(calibrator.getEstimatedMxy());
27611         assertNull(calibrator.getEstimatedMxz());
27612         assertNull(calibrator.getEstimatedMyx());
27613         assertNull(calibrator.getEstimatedMyz());
27614         assertNull(calibrator.getEstimatedMzx());
27615         assertNull(calibrator.getEstimatedMzy());
27616         assertNull(calibrator.getEstimatedCovariance());
27617         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27618         assertNotNull(calibrator.getGroundTruthGravityNorm());
27619         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27620         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27621         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27622                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27623         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27624         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27625         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27626 
27627         // Force IllegalArgumentException
27628         final Acceleration invalidGravityNorm = new Acceleration(
27629                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27630 
27631         calibrator = null;
27632         try {
27633             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27634                     invalidGravityNorm, ba, ma);
27635             fail("IllegalArgumentException expected but not thrown");
27636         } catch (final IllegalArgumentException ignore) {
27637         }
27638         try {
27639             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27640                     gravityNorm, new Matrix(1, 1), ma);
27641             fail("IllegalArgumentException expected but not thrown");
27642         } catch (final IllegalArgumentException ignore) {
27643         }
27644         try {
27645             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27646                     gravityNorm, new Matrix(1, 3), ma);
27647             fail("IllegalArgumentException expected but not thrown");
27648         } catch (final IllegalArgumentException ignore) {
27649         }
27650         try {
27651             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27652                     gravityNorm, ba, new Matrix(1, 3));
27653             fail("IllegalArgumentException expected but not thrown");
27654         } catch (final IllegalArgumentException ignore) {
27655         }
27656         try {
27657             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27658                     gravityNorm,
27659                     ba, new Matrix(3, 1));
27660             fail("IllegalArgumentException expected but not thrown");
27661         } catch (final IllegalArgumentException ignore) {
27662         }
27663         assertNull(calibrator);
27664     }
27665 
27666     @Test
27667     public void testConstructor228() throws WrongSizeException {
27668         final Matrix ba = generateBa();
27669         final double[] bias = ba.getBuffer();
27670         final double biasX = ba.getElementAtIndex(0);
27671         final double biasY = ba.getElementAtIndex(1);
27672         final double biasZ = ba.getElementAtIndex(2);
27673 
27674         final Matrix ma = generateMaCommonAxis();
27675         final double sx = ma.getElementAt(0, 0);
27676         final double sy = ma.getElementAt(1, 1);
27677         final double sz = ma.getElementAt(2, 2);
27678         final double mxy = ma.getElementAt(0, 1);
27679         final double mxz = ma.getElementAt(0, 2);
27680         final double myx = ma.getElementAt(1, 0);
27681         final double myz = ma.getElementAt(1, 2);
27682         final double mzx = ma.getElementAt(2, 0);
27683         final double mzy = ma.getElementAt(2, 1);
27684 
27685         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27686         final double latitude = Math.toRadians(
27687                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27688         final double longitude = Math.toRadians(
27689                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27690         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27691         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27692         final NEDVelocity nedVelocity = new NEDVelocity();
27693         final ECEFPosition ecefPosition = new ECEFPosition();
27694         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27695         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27696                 ecefPosition, ecefVelocity);
27697         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27698                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27699         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27700 
27701         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
27702                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
27703                         ba, ma, this);
27704 
27705         // check default values
27706         assertEquals(calibrator.getBiasX(), biasX, 0.0);
27707         assertEquals(calibrator.getBiasY(), biasY, 0.0);
27708         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
27709         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
27710         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27711         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27712         final Acceleration bx2 = new Acceleration(0.0,
27713                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27714         calibrator.getBiasXAsAcceleration(bx2);
27715         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27716         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27717         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
27718         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27719         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27720         final Acceleration by2 = new Acceleration(0.0,
27721                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27722         calibrator.getBiasYAsAcceleration(by2);
27723         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27724         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27725         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
27726         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27727         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27728         final Acceleration bz2 = new Acceleration(0.0,
27729                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27730         calibrator.getBiasZAsAcceleration(bz2);
27731         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27732         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27733         assertEquals(calibrator.getInitialSx(), sx, 0.0);
27734         assertEquals(calibrator.getInitialSy(), sy, 0.0);
27735         assertEquals(calibrator.getInitialSz(), sz, 0.0);
27736         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27737         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27738         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27739         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27740         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27741         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27742         final double[] bias1 = calibrator.getBias();
27743         assertArrayEquals(bias1, bias, 0.0);
27744         final double[] bias2 = new double[3];
27745         calibrator.getBias(bias2);
27746         assertArrayEquals(bias1, bias2, 0.0);
27747         final Matrix b1 = calibrator.getBiasAsMatrix();
27748         assertEquals(b1, ba);
27749         final Matrix b2 = new Matrix(3, 1);
27750         calibrator.getBiasAsMatrix(b2);
27751         assertEquals(b1, b2);
27752         final Matrix ma1 = new Matrix(3, 3);
27753         ma1.setSubmatrix(0, 0,
27754                 2, 2,
27755                 new double[]{sx, myx, mzx,
27756                         mxy, sy, mzy,
27757                         mxz, myz, sz});
27758         assertEquals(calibrator.getInitialMa(), ma1);
27759         final Matrix ma2 = new Matrix(3, 3);
27760         calibrator.getInitialMa(ma2);
27761         assertEquals(ma1, ma2);
27762         assertNull(calibrator.getMeasurements());
27763         assertFalse(calibrator.isCommonAxisUsed());
27764         assertSame(calibrator.getListener(), this);
27765         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27766         assertFalse(calibrator.isReady());
27767         assertFalse(calibrator.isRunning());
27768         assertNull(calibrator.getEstimatedMa());
27769         assertNull(calibrator.getEstimatedSx());
27770         assertNull(calibrator.getEstimatedSy());
27771         assertNull(calibrator.getEstimatedSz());
27772         assertNull(calibrator.getEstimatedMxy());
27773         assertNull(calibrator.getEstimatedMxz());
27774         assertNull(calibrator.getEstimatedMyx());
27775         assertNull(calibrator.getEstimatedMyz());
27776         assertNull(calibrator.getEstimatedMzx());
27777         assertNull(calibrator.getEstimatedMzy());
27778         assertNull(calibrator.getEstimatedCovariance());
27779         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27780         assertNotNull(calibrator.getGroundTruthGravityNorm());
27781         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27782         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27783         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27784                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27785         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27786         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27787         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27788 
27789         // Force IllegalArgumentException
27790         final Acceleration invalidGravityNorm = new Acceleration(
27791                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27792 
27793         calibrator = null;
27794         try {
27795             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27796                     invalidGravityNorm, ba, ma, this);
27797             fail("IllegalArgumentException expected but not thrown");
27798         } catch (final IllegalArgumentException ignore) {
27799         }
27800         try {
27801             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27802                     gravityNorm, new Matrix(1, 1), ma,
27803                     this);
27804             fail("IllegalArgumentException expected but not thrown");
27805         } catch (final IllegalArgumentException ignore) {
27806         }
27807         try {
27808             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27809                     gravityNorm, new Matrix(1, 3), ma,
27810                     this);
27811             fail("IllegalArgumentException expected but not thrown");
27812         } catch (final IllegalArgumentException ignore) {
27813         }
27814         try {
27815             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27816                     gravityNorm, ba, new Matrix(1, 3),
27817                     this);
27818             fail("IllegalArgumentException expected but not thrown");
27819         } catch (final IllegalArgumentException ignore) {
27820         }
27821         try {
27822             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27823                     gravityNorm, ba, new Matrix(3, 1),
27824                     this);
27825             fail("IllegalArgumentException expected but not thrown");
27826         } catch (final IllegalArgumentException ignore) {
27827         }
27828         assertNull(calibrator);
27829     }
27830 
27831     @Test
27832     public void testConstructor229() throws WrongSizeException {
27833         final Collection<StandardDeviationBodyKinematics> measurements =
27834                 Collections.emptyList();
27835 
27836         final Matrix ba = generateBa();
27837         final double[] bias = ba.getBuffer();
27838         final double biasX = ba.getElementAtIndex(0);
27839         final double biasY = ba.getElementAtIndex(1);
27840         final double biasZ = ba.getElementAtIndex(2);
27841 
27842         final Matrix ma = generateMaCommonAxis();
27843         final double sx = ma.getElementAt(0, 0);
27844         final double sy = ma.getElementAt(1, 1);
27845         final double sz = ma.getElementAt(2, 2);
27846         final double mxy = ma.getElementAt(0, 1);
27847         final double mxz = ma.getElementAt(0, 2);
27848         final double myx = ma.getElementAt(1, 0);
27849         final double myz = ma.getElementAt(1, 2);
27850         final double mzx = ma.getElementAt(2, 0);
27851         final double mzy = ma.getElementAt(2, 1);
27852 
27853         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27854         final double latitude = Math.toRadians(
27855                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27856         final double longitude = Math.toRadians(
27857                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27858         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27859         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27860         final NEDVelocity nedVelocity = new NEDVelocity();
27861         final ECEFPosition ecefPosition = new ECEFPosition();
27862         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27863         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27864                 ecefPosition, ecefVelocity);
27865         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27866                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27867         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27868 
27869         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
27870                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
27871                         measurements, ba, ma);
27872 
27873         // check default values
27874         assertEquals(calibrator.getBiasX(), biasX, 0.0);
27875         assertEquals(calibrator.getBiasY(), biasY, 0.0);
27876         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
27877         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
27878         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27879         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27880         final Acceleration bx2 = new Acceleration(0.0,
27881                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27882         calibrator.getBiasXAsAcceleration(bx2);
27883         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27884         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27885         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
27886         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27887         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27888         final Acceleration by2 = new Acceleration(0.0,
27889                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27890         calibrator.getBiasYAsAcceleration(by2);
27891         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27892         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27893         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
27894         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27895         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27896         final Acceleration bz2 = new Acceleration(0.0,
27897                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27898         calibrator.getBiasZAsAcceleration(bz2);
27899         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27900         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27901         assertEquals(calibrator.getInitialSx(), sx, 0.0);
27902         assertEquals(calibrator.getInitialSy(), sy, 0.0);
27903         assertEquals(calibrator.getInitialSz(), sz, 0.0);
27904         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27905         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27906         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27907         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27908         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27909         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27910         final double[] bias1 = calibrator.getBias();
27911         assertArrayEquals(bias1, bias, 0.0);
27912         final double[] bias2 = new double[3];
27913         calibrator.getBias(bias2);
27914         assertArrayEquals(bias1, bias2, 0.0);
27915         final Matrix b1 = calibrator.getBiasAsMatrix();
27916         assertEquals(b1, ba);
27917         final Matrix b2 = new Matrix(3, 1);
27918         calibrator.getBiasAsMatrix(b2);
27919         assertEquals(b1, b2);
27920         final Matrix ma1 = new Matrix(3, 3);
27921         ma1.setSubmatrix(0, 0,
27922                 2, 2,
27923                 new double[]{sx, myx, mzx,
27924                         mxy, sy, mzy,
27925                         mxz, myz, sz});
27926         assertEquals(calibrator.getInitialMa(), ma1);
27927         final Matrix ma2 = new Matrix(3, 3);
27928         calibrator.getInitialMa(ma2);
27929         assertEquals(ma1, ma2);
27930         assertSame(calibrator.getMeasurements(), measurements);
27931         assertFalse(calibrator.isCommonAxisUsed());
27932         assertNull(calibrator.getListener());
27933         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27934         assertFalse(calibrator.isReady());
27935         assertFalse(calibrator.isRunning());
27936         assertNull(calibrator.getEstimatedMa());
27937         assertNull(calibrator.getEstimatedSx());
27938         assertNull(calibrator.getEstimatedSy());
27939         assertNull(calibrator.getEstimatedSz());
27940         assertNull(calibrator.getEstimatedMxy());
27941         assertNull(calibrator.getEstimatedMxz());
27942         assertNull(calibrator.getEstimatedMyx());
27943         assertNull(calibrator.getEstimatedMyz());
27944         assertNull(calibrator.getEstimatedMzx());
27945         assertNull(calibrator.getEstimatedMzy());
27946         assertNull(calibrator.getEstimatedCovariance());
27947         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27948         assertNotNull(calibrator.getGroundTruthGravityNorm());
27949         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27950         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27951         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27952                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27953         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27954         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27955         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27956 
27957         // Force IllegalArgumentException
27958         final Acceleration invalidGravityNorm = new Acceleration(
27959                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27960 
27961         calibrator = null;
27962         try {
27963             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27964                     invalidGravityNorm, ba, ma, this);
27965             fail("IllegalArgumentException expected but not thrown");
27966         } catch (final IllegalArgumentException ignore) {
27967         }
27968         try {
27969             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27970                     gravityNorm, measurements,
27971                     new Matrix(1, 1), ma);
27972             fail("IllegalArgumentException expected but not thrown");
27973         } catch (final IllegalArgumentException ignore) {
27974         }
27975         try {
27976             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27977                     gravityNorm, measurements,
27978                     new Matrix(1, 3), ma);
27979             fail("IllegalArgumentException expected but not thrown");
27980         } catch (final IllegalArgumentException ignore) {
27981         }
27982         try {
27983             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27984                     gravityNorm, measurements, ba,
27985                     new Matrix(1, 3));
27986             fail("IllegalArgumentException expected but not thrown");
27987         } catch (final IllegalArgumentException ignore) {
27988         }
27989         try {
27990             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
27991                     gravityNorm, measurements, ba,
27992                     new Matrix(3, 1));
27993             fail("IllegalArgumentException expected but not thrown");
27994         } catch (final IllegalArgumentException ignore) {
27995         }
27996         assertNull(calibrator);
27997     }
27998 
27999     @Test
28000     public void testConstructor230() throws WrongSizeException {
28001         final Collection<StandardDeviationBodyKinematics> measurements =
28002                 Collections.emptyList();
28003 
28004         final Matrix ba = generateBa();
28005         final double[] bias = ba.getBuffer();
28006         final double biasX = ba.getElementAtIndex(0);
28007         final double biasY = ba.getElementAtIndex(1);
28008         final double biasZ = ba.getElementAtIndex(2);
28009 
28010         final Matrix ma = generateMaCommonAxis();
28011         final double sx = ma.getElementAt(0, 0);
28012         final double sy = ma.getElementAt(1, 1);
28013         final double sz = ma.getElementAt(2, 2);
28014         final double mxy = ma.getElementAt(0, 1);
28015         final double mxz = ma.getElementAt(0, 2);
28016         final double myx = ma.getElementAt(1, 0);
28017         final double myz = ma.getElementAt(1, 2);
28018         final double mzx = ma.getElementAt(2, 0);
28019         final double mzy = ma.getElementAt(2, 1);
28020 
28021         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28022         final double latitude = Math.toRadians(
28023                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28024         final double longitude = Math.toRadians(
28025                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28026         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28027         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28028         final NEDVelocity nedVelocity = new NEDVelocity();
28029         final ECEFPosition ecefPosition = new ECEFPosition();
28030         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28031         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28032                 ecefPosition, ecefVelocity);
28033         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28034                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28035         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28036 
28037         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28038                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
28039                         measurements, ba, ma, this);
28040 
28041         // check default values
28042         assertEquals(calibrator.getBiasX(), biasX, 0.0);
28043         assertEquals(calibrator.getBiasY(), biasY, 0.0);
28044         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
28045         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
28046         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28047         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28048         final Acceleration bx2 = new Acceleration(0.0,
28049                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28050         calibrator.getBiasXAsAcceleration(bx2);
28051         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28052         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28053         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
28054         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28055         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28056         final Acceleration by2 = new Acceleration(0.0,
28057                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28058         calibrator.getBiasYAsAcceleration(by2);
28059         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28060         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28061         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
28062         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28063         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28064         final Acceleration bz2 = new Acceleration(0.0,
28065                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28066         calibrator.getBiasZAsAcceleration(bz2);
28067         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28068         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28069         assertEquals(calibrator.getInitialSx(), sx, 0.0);
28070         assertEquals(calibrator.getInitialSy(), sy, 0.0);
28071         assertEquals(calibrator.getInitialSz(), sz, 0.0);
28072         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28073         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28074         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28075         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28076         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28077         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28078         final double[] bias1 = calibrator.getBias();
28079         assertArrayEquals(bias1, bias, 0.0);
28080         final double[] bias2 = new double[3];
28081         calibrator.getBias(bias2);
28082         assertArrayEquals(bias1, bias2, 0.0);
28083         final Matrix b1 = calibrator.getBiasAsMatrix();
28084         assertEquals(b1, ba);
28085         final Matrix b2 = new Matrix(3, 1);
28086         calibrator.getBiasAsMatrix(b2);
28087         assertEquals(b1, b2);
28088         final Matrix ma1 = new Matrix(3, 3);
28089         ma1.setSubmatrix(0, 0,
28090                 2, 2,
28091                 new double[]{sx, myx, mzx,
28092                         mxy, sy, mzy,
28093                         mxz, myz, sz});
28094         assertEquals(calibrator.getInitialMa(), ma1);
28095         final Matrix ma2 = new Matrix(3, 3);
28096         calibrator.getInitialMa(ma2);
28097         assertEquals(ma1, ma2);
28098         assertSame(calibrator.getMeasurements(), measurements);
28099         assertFalse(calibrator.isCommonAxisUsed());
28100         assertSame(calibrator.getListener(), this);
28101         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28102         assertFalse(calibrator.isReady());
28103         assertFalse(calibrator.isRunning());
28104         assertNull(calibrator.getEstimatedMa());
28105         assertNull(calibrator.getEstimatedSx());
28106         assertNull(calibrator.getEstimatedSy());
28107         assertNull(calibrator.getEstimatedSz());
28108         assertNull(calibrator.getEstimatedMxy());
28109         assertNull(calibrator.getEstimatedMxz());
28110         assertNull(calibrator.getEstimatedMyx());
28111         assertNull(calibrator.getEstimatedMyz());
28112         assertNull(calibrator.getEstimatedMzx());
28113         assertNull(calibrator.getEstimatedMzy());
28114         assertNull(calibrator.getEstimatedCovariance());
28115         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28116         assertNotNull(calibrator.getGroundTruthGravityNorm());
28117         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28118         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28119         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28120                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28121         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28122         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28123         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28124 
28125         // Force IllegalArgumentException
28126         final Acceleration invalidGravityNorm = new Acceleration(
28127                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28128 
28129         calibrator = null;
28130         try {
28131             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28132                     invalidGravityNorm, ba, ma, this);
28133             fail("IllegalArgumentException expected but not thrown");
28134         } catch (final IllegalArgumentException ignore) {
28135         }
28136         try {
28137             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28138                     gravityNorm, measurements,
28139                     new Matrix(1, 1), ma, this);
28140             fail("IllegalArgumentException expected but not thrown");
28141         } catch (final IllegalArgumentException ignore) {
28142         }
28143         try {
28144             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28145                     gravityNorm, measurements,
28146                     new Matrix(1, 3), ma, this);
28147             fail("IllegalArgumentException expected but not thrown");
28148         } catch (final IllegalArgumentException ignore) {
28149         }
28150         try {
28151             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28152                     gravityNorm, measurements, ba,
28153                     new Matrix(1, 3), this);
28154             fail("IllegalArgumentException expected but not thrown");
28155         } catch (final IllegalArgumentException ignore) {
28156         }
28157         try {
28158             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28159                     gravityNorm, measurements, ba,
28160                     new Matrix(3, 1), this);
28161             fail("IllegalArgumentException expected but not thrown");
28162         } catch (final IllegalArgumentException ignore) {
28163         }
28164         assertNull(calibrator);
28165     }
28166 
28167     @Test
28168     public void testConstructor231() throws WrongSizeException {
28169         final Matrix ba = generateBa();
28170         final double[] bias = ba.getBuffer();
28171         final double biasX = ba.getElementAtIndex(0);
28172         final double biasY = ba.getElementAtIndex(1);
28173         final double biasZ = ba.getElementAtIndex(2);
28174 
28175         final Matrix ma = generateMaCommonAxis();
28176         final double sx = ma.getElementAt(0, 0);
28177         final double sy = ma.getElementAt(1, 1);
28178         final double sz = ma.getElementAt(2, 2);
28179         final double mxy = ma.getElementAt(0, 1);
28180         final double mxz = ma.getElementAt(0, 2);
28181         final double myx = ma.getElementAt(1, 0);
28182         final double myz = ma.getElementAt(1, 2);
28183         final double mzx = ma.getElementAt(2, 0);
28184         final double mzy = ma.getElementAt(2, 1);
28185 
28186         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28187         final double latitude = Math.toRadians(
28188                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28189         final double longitude = Math.toRadians(
28190                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28191         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28192         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28193         final NEDVelocity nedVelocity = new NEDVelocity();
28194         final ECEFPosition ecefPosition = new ECEFPosition();
28195         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28196         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28197                 ecefPosition, ecefVelocity);
28198         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28199                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28200         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28201 
28202         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28203                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
28204                         true, ba, ma);
28205 
28206         // check default values
28207         assertEquals(calibrator.getBiasX(), biasX, 0.0);
28208         assertEquals(calibrator.getBiasY(), biasY, 0.0);
28209         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
28210         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
28211         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28212         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28213         final Acceleration bx2 = new Acceleration(0.0,
28214                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28215         calibrator.getBiasXAsAcceleration(bx2);
28216         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28217         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28218         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
28219         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28220         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28221         final Acceleration by2 = new Acceleration(0.0,
28222                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28223         calibrator.getBiasYAsAcceleration(by2);
28224         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28225         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28226         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
28227         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28228         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28229         final Acceleration bz2 = new Acceleration(0.0,
28230                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28231         calibrator.getBiasZAsAcceleration(bz2);
28232         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28233         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28234         assertEquals(calibrator.getInitialSx(), sx, 0.0);
28235         assertEquals(calibrator.getInitialSy(), sy, 0.0);
28236         assertEquals(calibrator.getInitialSz(), sz, 0.0);
28237         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28238         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28239         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28240         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28241         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28242         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28243         final double[] bias1 = calibrator.getBias();
28244         assertArrayEquals(bias1, bias, 0.0);
28245         final double[] bias2 = new double[3];
28246         calibrator.getBias(bias2);
28247         assertArrayEquals(bias1, bias2, 0.0);
28248         final Matrix b1 = calibrator.getBiasAsMatrix();
28249         assertEquals(b1, ba);
28250         final Matrix b2 = new Matrix(3, 1);
28251         calibrator.getBiasAsMatrix(b2);
28252         assertEquals(b1, b2);
28253         final Matrix ma1 = new Matrix(3, 3);
28254         ma1.setSubmatrix(0, 0,
28255                 2, 2,
28256                 new double[]{sx, myx, mzx,
28257                         mxy, sy, mzy,
28258                         mxz, myz, sz});
28259         assertEquals(calibrator.getInitialMa(), ma1);
28260         final Matrix ma2 = new Matrix(3, 3);
28261         calibrator.getInitialMa(ma2);
28262         assertEquals(ma1, ma2);
28263         assertNull(calibrator.getMeasurements());
28264         assertTrue(calibrator.isCommonAxisUsed());
28265         assertNull(calibrator.getListener());
28266         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
28267         assertFalse(calibrator.isReady());
28268         assertFalse(calibrator.isRunning());
28269         assertNull(calibrator.getEstimatedMa());
28270         assertNull(calibrator.getEstimatedSx());
28271         assertNull(calibrator.getEstimatedSy());
28272         assertNull(calibrator.getEstimatedSz());
28273         assertNull(calibrator.getEstimatedMxy());
28274         assertNull(calibrator.getEstimatedMxz());
28275         assertNull(calibrator.getEstimatedMyx());
28276         assertNull(calibrator.getEstimatedMyz());
28277         assertNull(calibrator.getEstimatedMzx());
28278         assertNull(calibrator.getEstimatedMzy());
28279         assertNull(calibrator.getEstimatedCovariance());
28280         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28281         assertNotNull(calibrator.getGroundTruthGravityNorm());
28282         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28283         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28284         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28285                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28286         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28287         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28288         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28289 
28290         // Force IllegalArgumentException
28291         final Acceleration invalidGravityNorm = new Acceleration(
28292                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28293 
28294         calibrator = null;
28295         try {
28296             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28297                     invalidGravityNorm, true, ba, ma);
28298             fail("IllegalArgumentException expected but not thrown");
28299         } catch (final IllegalArgumentException ignore) {
28300         }
28301         try {
28302             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28303                     gravityNorm, true,
28304                     new Matrix(1, 1), ma);
28305             fail("IllegalArgumentException expected but not thrown");
28306         } catch (final IllegalArgumentException ignore) {
28307         }
28308         try {
28309             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28310                     gravityNorm, true,
28311                     new Matrix(1, 3), ma);
28312             fail("IllegalArgumentException expected but not thrown");
28313         } catch (final IllegalArgumentException ignore) {
28314         }
28315         try {
28316             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28317                     gravityNorm, true, ba,
28318                     new Matrix(1, 3));
28319             fail("IllegalArgumentException expected but not thrown");
28320         } catch (final IllegalArgumentException ignore) {
28321         }
28322         try {
28323             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28324                     gravityNorm, true, ba,
28325                     new Matrix(3, 1));
28326             fail("IllegalArgumentException expected but not thrown");
28327         } catch (final IllegalArgumentException ignore) {
28328         }
28329         assertNull(calibrator);
28330     }
28331 
28332     @Test
28333     public void testConstructor232() throws WrongSizeException {
28334         final Matrix ba = generateBa();
28335         final double[] bias = ba.getBuffer();
28336         final double biasX = ba.getElementAtIndex(0);
28337         final double biasY = ba.getElementAtIndex(1);
28338         final double biasZ = ba.getElementAtIndex(2);
28339 
28340         final Matrix ma = generateMaCommonAxis();
28341         final double sx = ma.getElementAt(0, 0);
28342         final double sy = ma.getElementAt(1, 1);
28343         final double sz = ma.getElementAt(2, 2);
28344         final double mxy = ma.getElementAt(0, 1);
28345         final double mxz = ma.getElementAt(0, 2);
28346         final double myx = ma.getElementAt(1, 0);
28347         final double myz = ma.getElementAt(1, 2);
28348         final double mzx = ma.getElementAt(2, 0);
28349         final double mzy = ma.getElementAt(2, 1);
28350 
28351         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28352         final double latitude = Math.toRadians(
28353                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28354         final double longitude = Math.toRadians(
28355                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28356         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28357         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28358         final NEDVelocity nedVelocity = new NEDVelocity();
28359         final ECEFPosition ecefPosition = new ECEFPosition();
28360         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28361         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28362                 ecefPosition, ecefVelocity);
28363         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28364                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28365         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28366 
28367         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28368                 new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm,
28369                         true, ba, ma, this);
28370 
28371         // check default values
28372         assertEquals(calibrator.getBiasX(), biasX, 0.0);
28373         assertEquals(calibrator.getBiasY(), biasY, 0.0);
28374         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
28375         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
28376         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28377         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28378         final Acceleration bx2 = new Acceleration(0.0,
28379                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28380         calibrator.getBiasXAsAcceleration(bx2);
28381         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28382         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28383         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
28384         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28385         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28386         final Acceleration by2 = new Acceleration(0.0,
28387                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28388         calibrator.getBiasYAsAcceleration(by2);
28389         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28390         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28391         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
28392         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28393         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28394         final Acceleration bz2 = new Acceleration(0.0,
28395                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28396         calibrator.getBiasZAsAcceleration(bz2);
28397         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28398         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28399         assertEquals(calibrator.getInitialSx(), sx, 0.0);
28400         assertEquals(calibrator.getInitialSy(), sy, 0.0);
28401         assertEquals(calibrator.getInitialSz(), sz, 0.0);
28402         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28403         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28404         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28405         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28406         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28407         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28408         final double[] bias1 = calibrator.getBias();
28409         assertArrayEquals(bias1, bias, 0.0);
28410         final double[] bias2 = new double[3];
28411         calibrator.getBias(bias2);
28412         assertArrayEquals(bias1, bias2, 0.0);
28413         final Matrix b1 = calibrator.getBiasAsMatrix();
28414         assertEquals(b1, ba);
28415         final Matrix b2 = new Matrix(3, 1);
28416         calibrator.getBiasAsMatrix(b2);
28417         assertEquals(b1, b2);
28418         final Matrix ma1 = new Matrix(3, 3);
28419         ma1.setSubmatrix(0, 0,
28420                 2, 2,
28421                 new double[]{sx, myx, mzx,
28422                         mxy, sy, mzy,
28423                         mxz, myz, sz});
28424         assertEquals(calibrator.getInitialMa(), ma1);
28425         final Matrix ma2 = new Matrix(3, 3);
28426         calibrator.getInitialMa(ma2);
28427         assertEquals(ma1, ma2);
28428         assertNull(calibrator.getMeasurements());
28429         assertTrue(calibrator.isCommonAxisUsed());
28430         assertSame(calibrator.getListener(), this);
28431         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
28432         assertFalse(calibrator.isReady());
28433         assertFalse(calibrator.isRunning());
28434         assertNull(calibrator.getEstimatedMa());
28435         assertNull(calibrator.getEstimatedSx());
28436         assertNull(calibrator.getEstimatedSy());
28437         assertNull(calibrator.getEstimatedSz());
28438         assertNull(calibrator.getEstimatedMxy());
28439         assertNull(calibrator.getEstimatedMxz());
28440         assertNull(calibrator.getEstimatedMyx());
28441         assertNull(calibrator.getEstimatedMyz());
28442         assertNull(calibrator.getEstimatedMzx());
28443         assertNull(calibrator.getEstimatedMzy());
28444         assertNull(calibrator.getEstimatedCovariance());
28445         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28446         assertNotNull(calibrator.getGroundTruthGravityNorm());
28447         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28448         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28449         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28450                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28451         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28452         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28453         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28454 
28455         // Force IllegalArgumentException
28456         final Acceleration invalidGravityNorm = new Acceleration(
28457                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28458 
28459         calibrator = null;
28460         try {
28461             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28462                     invalidGravityNorm, true, ba, ma);
28463             fail("IllegalArgumentException expected but not thrown");
28464         } catch (final IllegalArgumentException ignore) {
28465         }
28466         try {
28467             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28468                     gravityNorm, true,
28469                     new Matrix(1, 1), ma, this);
28470             fail("IllegalArgumentException expected but not thrown");
28471         } catch (final IllegalArgumentException ignore) {
28472         }
28473         try {
28474             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28475                     gravityNorm, true,
28476                     new Matrix(1, 3), ma, this);
28477             fail("IllegalArgumentException expected but not thrown");
28478         } catch (final IllegalArgumentException ignore) {
28479         }
28480         try {
28481             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28482                     gravityNorm, true, ba,
28483                     new Matrix(1, 3), this);
28484             fail("IllegalArgumentException expected but not thrown");
28485         } catch (final IllegalArgumentException ignore) {
28486         }
28487         try {
28488             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28489                     gravityNorm, true, ba,
28490                     new Matrix(3, 1), this);
28491             fail("IllegalArgumentException expected but not thrown");
28492         } catch (final IllegalArgumentException ignore) {
28493         }
28494         assertNull(calibrator);
28495     }
28496 
28497     @Test
28498     public void testConstructor233() throws WrongSizeException {
28499         final Collection<StandardDeviationBodyKinematics> measurements =
28500                 Collections.emptyList();
28501 
28502         final Matrix ba = generateBa();
28503         final double[] bias = ba.getBuffer();
28504         final double biasX = ba.getElementAtIndex(0);
28505         final double biasY = ba.getElementAtIndex(1);
28506         final double biasZ = ba.getElementAtIndex(2);
28507 
28508         final Matrix ma = generateMaCommonAxis();
28509         final double sx = ma.getElementAt(0, 0);
28510         final double sy = ma.getElementAt(1, 1);
28511         final double sz = ma.getElementAt(2, 2);
28512         final double mxy = ma.getElementAt(0, 1);
28513         final double mxz = ma.getElementAt(0, 2);
28514         final double myx = ma.getElementAt(1, 0);
28515         final double myz = ma.getElementAt(1, 2);
28516         final double mzx = ma.getElementAt(2, 0);
28517         final double mzy = ma.getElementAt(2, 1);
28518 
28519         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28520         final double latitude = Math.toRadians(
28521                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28522         final double longitude = Math.toRadians(
28523                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28524         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28525         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28526         final NEDVelocity nedVelocity = new NEDVelocity();
28527         final ECEFPosition ecefPosition = new ECEFPosition();
28528         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28529         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28530                 ecefPosition, ecefVelocity);
28531         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28532                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28533         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28534 
28535         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28536                 new KnownBiasAndGravityNormAccelerometerCalibrator(
28537                         gravityNorm, measurements, true,
28538                         ba, ma);
28539 
28540         // check default values
28541         assertEquals(calibrator.getBiasX(), biasX, 0.0);
28542         assertEquals(calibrator.getBiasY(), biasY, 0.0);
28543         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
28544         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
28545         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28546         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28547         final Acceleration bx2 = new Acceleration(0.0,
28548                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28549         calibrator.getBiasXAsAcceleration(bx2);
28550         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28551         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28552         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
28553         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28554         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28555         final Acceleration by2 = new Acceleration(0.0,
28556                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28557         calibrator.getBiasYAsAcceleration(by2);
28558         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28559         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28560         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
28561         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28562         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28563         final Acceleration bz2 = new Acceleration(0.0,
28564                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28565         calibrator.getBiasZAsAcceleration(bz2);
28566         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28567         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28568         assertEquals(calibrator.getInitialSx(), sx, 0.0);
28569         assertEquals(calibrator.getInitialSy(), sy, 0.0);
28570         assertEquals(calibrator.getInitialSz(), sz, 0.0);
28571         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28572         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28573         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28574         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28575         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28576         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28577         final double[] bias1 = calibrator.getBias();
28578         assertArrayEquals(bias1, bias, 0.0);
28579         final double[] bias2 = new double[3];
28580         calibrator.getBias(bias2);
28581         assertArrayEquals(bias1, bias2, 0.0);
28582         final Matrix b1 = calibrator.getBiasAsMatrix();
28583         assertEquals(b1, ba);
28584         final Matrix b2 = new Matrix(3, 1);
28585         calibrator.getBiasAsMatrix(b2);
28586         assertEquals(b1, b2);
28587         final Matrix ma1 = new Matrix(3, 3);
28588         ma1.setSubmatrix(0, 0,
28589                 2, 2,
28590                 new double[]{sx, myx, mzx,
28591                         mxy, sy, mzy,
28592                         mxz, myz, sz});
28593         assertEquals(calibrator.getInitialMa(), ma1);
28594         final Matrix ma2 = new Matrix(3, 3);
28595         calibrator.getInitialMa(ma2);
28596         assertEquals(ma1, ma2);
28597         assertSame(calibrator.getMeasurements(), measurements);
28598         assertTrue(calibrator.isCommonAxisUsed());
28599         assertNull(calibrator.getListener());
28600         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
28601         assertFalse(calibrator.isReady());
28602         assertFalse(calibrator.isRunning());
28603         assertNull(calibrator.getEstimatedMa());
28604         assertNull(calibrator.getEstimatedSx());
28605         assertNull(calibrator.getEstimatedSy());
28606         assertNull(calibrator.getEstimatedSz());
28607         assertNull(calibrator.getEstimatedMxy());
28608         assertNull(calibrator.getEstimatedMxz());
28609         assertNull(calibrator.getEstimatedMyx());
28610         assertNull(calibrator.getEstimatedMyz());
28611         assertNull(calibrator.getEstimatedMzx());
28612         assertNull(calibrator.getEstimatedMzy());
28613         assertNull(calibrator.getEstimatedCovariance());
28614         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28615         assertNotNull(calibrator.getGroundTruthGravityNorm());
28616         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28617         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28618         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28619                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28620         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28621         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28622         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28623 
28624         // Force IllegalArgumentException
28625         final Acceleration invalidGravityNorm = new Acceleration(
28626                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28627 
28628         calibrator = null;
28629         try {
28630             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28631                     invalidGravityNorm, measurements, true,
28632                     ba, ma);
28633             fail("IllegalArgumentException expected but not thrown");
28634         } catch (final IllegalArgumentException ignore) {
28635         }
28636         try {
28637             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28638                     gravityNorm, measurements, true,
28639                     new Matrix(1, 1), ma);
28640             fail("IllegalArgumentException expected but not thrown");
28641         } catch (final IllegalArgumentException ignore) {
28642         }
28643         try {
28644             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28645                     gravityNorm, measurements, true,
28646                     new Matrix(1, 3), ma);
28647             fail("IllegalArgumentException expected but not thrown");
28648         } catch (final IllegalArgumentException ignore) {
28649         }
28650         try {
28651             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28652                     gravityNorm, measurements, true,
28653                     ba, new Matrix(1, 3));
28654             fail("IllegalArgumentException expected but not thrown");
28655         } catch (final IllegalArgumentException ignore) {
28656         }
28657         try {
28658             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28659                     gravityNorm, measurements, true,
28660                     ba, new Matrix(3, 1));
28661             fail("IllegalArgumentException expected but not thrown");
28662         } catch (final IllegalArgumentException ignore) {
28663         }
28664         assertNull(calibrator);
28665     }
28666 
28667     @Test
28668     public void testConstructor234() throws WrongSizeException {
28669         final Collection<StandardDeviationBodyKinematics> measurements =
28670                 Collections.emptyList();
28671 
28672         final Matrix ba = generateBa();
28673         final double[] bias = ba.getBuffer();
28674         final double biasX = ba.getElementAtIndex(0);
28675         final double biasY = ba.getElementAtIndex(1);
28676         final double biasZ = ba.getElementAtIndex(2);
28677 
28678         final Matrix ma = generateMaCommonAxis();
28679         final double sx = ma.getElementAt(0, 0);
28680         final double sy = ma.getElementAt(1, 1);
28681         final double sz = ma.getElementAt(2, 2);
28682         final double mxy = ma.getElementAt(0, 1);
28683         final double mxz = ma.getElementAt(0, 2);
28684         final double myx = ma.getElementAt(1, 0);
28685         final double myz = ma.getElementAt(1, 2);
28686         final double mzx = ma.getElementAt(2, 0);
28687         final double mzy = ma.getElementAt(2, 1);
28688 
28689         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28690         final double latitude = Math.toRadians(
28691                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28692         final double longitude = Math.toRadians(
28693                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28694         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28695         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28696         final NEDVelocity nedVelocity = new NEDVelocity();
28697         final ECEFPosition ecefPosition = new ECEFPosition();
28698         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28699         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28700                 ecefPosition, ecefVelocity);
28701         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28702                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28703         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28704 
28705         KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28706                 new KnownBiasAndGravityNormAccelerometerCalibrator(
28707                         gravityNorm, measurements,
28708                         true, ba, ma, this);
28709 
28710         // check default values
28711         assertEquals(calibrator.getBiasX(), biasX, 0.0);
28712         assertEquals(calibrator.getBiasY(), biasY, 0.0);
28713         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
28714         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
28715         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28716         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28717         final Acceleration bx2 = new Acceleration(0.0,
28718                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28719         calibrator.getBiasXAsAcceleration(bx2);
28720         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28721         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28722         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
28723         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28724         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28725         final Acceleration by2 = new Acceleration(0.0,
28726                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28727         calibrator.getBiasYAsAcceleration(by2);
28728         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28729         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28730         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
28731         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28732         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28733         final Acceleration bz2 = new Acceleration(0.0,
28734                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28735         calibrator.getBiasZAsAcceleration(bz2);
28736         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28737         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28738         assertEquals(calibrator.getInitialSx(), sx, 0.0);
28739         assertEquals(calibrator.getInitialSy(), sy, 0.0);
28740         assertEquals(calibrator.getInitialSz(), sz, 0.0);
28741         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28742         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28743         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28744         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28745         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28746         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28747         final double[] bias1 = calibrator.getBias();
28748         assertArrayEquals(bias1, bias, 0.0);
28749         final double[] bias2 = new double[3];
28750         calibrator.getBias(bias2);
28751         assertArrayEquals(bias1, bias2, 0.0);
28752         final Matrix b1 = calibrator.getBiasAsMatrix();
28753         assertEquals(b1, ba);
28754         final Matrix b2 = new Matrix(3, 1);
28755         calibrator.getBiasAsMatrix(b2);
28756         assertEquals(b1, b2);
28757         final Matrix ma1 = new Matrix(3, 3);
28758         ma1.setSubmatrix(0, 0,
28759                 2, 2,
28760                 new double[]{sx, myx, mzx,
28761                         mxy, sy, mzy,
28762                         mxz, myz, sz});
28763         assertEquals(calibrator.getInitialMa(), ma1);
28764         final Matrix ma2 = new Matrix(3, 3);
28765         calibrator.getInitialMa(ma2);
28766         assertEquals(ma1, ma2);
28767         assertSame(calibrator.getMeasurements(), measurements);
28768         assertTrue(calibrator.isCommonAxisUsed());
28769         assertSame(calibrator.getListener(), this);
28770         assertEquals(calibrator.getMinimumRequiredMeasurements(), 7);
28771         assertFalse(calibrator.isReady());
28772         assertFalse(calibrator.isRunning());
28773         assertNull(calibrator.getEstimatedMa());
28774         assertNull(calibrator.getEstimatedSx());
28775         assertNull(calibrator.getEstimatedSy());
28776         assertNull(calibrator.getEstimatedSz());
28777         assertNull(calibrator.getEstimatedMxy());
28778         assertNull(calibrator.getEstimatedMxz());
28779         assertNull(calibrator.getEstimatedMyx());
28780         assertNull(calibrator.getEstimatedMyz());
28781         assertNull(calibrator.getEstimatedMzx());
28782         assertNull(calibrator.getEstimatedMzy());
28783         assertNull(calibrator.getEstimatedCovariance());
28784         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28785         assertNotNull(calibrator.getGroundTruthGravityNorm());
28786         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28787         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28788         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28789                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28790         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28791         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28792         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28793 
28794         // Force IllegalArgumentException
28795         final Acceleration invalidGravityNorm = new Acceleration(
28796                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28797 
28798         calibrator = null;
28799         try {
28800             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28801                     invalidGravityNorm, measurements,
28802                     true, ba, ma, this);
28803             fail("IllegalArgumentException expected but not thrown");
28804         } catch (final IllegalArgumentException ignore) {
28805         }
28806         try {
28807             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28808                     gravityNorm, measurements, true,
28809                     new Matrix(1, 1), ma, this);
28810             fail("IllegalArgumentException expected but not thrown");
28811         } catch (final IllegalArgumentException ignore) {
28812         }
28813         try {
28814             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28815                     gravityNorm, measurements, true,
28816                     new Matrix(1, 3), ma, this);
28817             fail("IllegalArgumentException expected but not thrown");
28818         } catch (final IllegalArgumentException ignore) {
28819         }
28820         try {
28821             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28822                     gravityNorm, measurements, true,
28823                     ba, new Matrix(1, 3), this);
28824             fail("IllegalArgumentException expected but not thrown");
28825         } catch (final IllegalArgumentException ignore) {
28826         }
28827         try {
28828             calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
28829                     gravityNorm, measurements, true,
28830                     ba, new Matrix(3, 1), this);
28831             fail("IllegalArgumentException expected but not thrown");
28832         } catch (final IllegalArgumentException ignore) {
28833         }
28834         assertNull(calibrator);
28835     }
28836 
28837     @Test
28838     public void testGetSetBiasX() throws LockedException {
28839         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28840                 new KnownBiasAndGravityNormAccelerometerCalibrator();
28841 
28842         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
28843 
28844         // set new value
28845         final Matrix ba = generateBa();
28846         final double biasX = ba.getElementAtIndex(0);
28847 
28848         calibrator.setBiasX(biasX);
28849 
28850         // check
28851         assertEquals(calibrator.getBiasX(), biasX, 0.0);
28852     }
28853 
28854     @Test
28855     public void testGetSetBiasY() throws LockedException {
28856         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28857                 new KnownBiasAndGravityNormAccelerometerCalibrator();
28858 
28859         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
28860 
28861         // set new value
28862         final Matrix ba = generateBa();
28863         final double biasY = ba.getElementAtIndex(1);
28864 
28865         calibrator.setBiasY(biasY);
28866 
28867         // check
28868         assertEquals(calibrator.getBiasY(), biasY, 0.0);
28869     }
28870 
28871     @Test
28872     public void testGetSetBiasZ() throws LockedException {
28873         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28874                 new KnownBiasAndGravityNormAccelerometerCalibrator();
28875 
28876         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
28877 
28878         // set new value
28879         final Matrix ba = generateBa();
28880         final double biasZ = ba.getElementAtIndex(2);
28881 
28882         calibrator.setBiasZ(biasZ);
28883 
28884         // check
28885         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
28886     }
28887 
28888     @Test
28889     public void testGetSetBiasXAsAcceleration() throws LockedException {
28890         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28891                 new KnownBiasAndGravityNormAccelerometerCalibrator();
28892 
28893         // check default value
28894         final Acceleration bx1 = calibrator.getBiasXAsAcceleration();
28895 
28896         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
28897         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28898 
28899         // set new value
28900         final Matrix ba = generateBa();
28901         final double biasX = ba.getElementAtIndex(0);
28902 
28903         final Acceleration bx2 = new Acceleration(biasX,
28904                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
28905         calibrator.setBiasX(bx2);
28906 
28907         // check
28908         final Acceleration bx3 = calibrator.getBiasXAsAcceleration();
28909         final Acceleration bx4 = new Acceleration(biasX,
28910                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
28911         calibrator.getBiasXAsAcceleration(bx4);
28912 
28913         assertEquals(bx3, bx4);
28914     }
28915 
28916     @Test
28917     public void testGetSetBiasYAsAcceleration() throws LockedException {
28918         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28919                 new KnownBiasAndGravityNormAccelerometerCalibrator();
28920 
28921         // check default value
28922         final Acceleration by1 = calibrator.getBiasYAsAcceleration();
28923 
28924         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
28925         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28926 
28927         // set new value
28928         final Matrix ba = generateBa();
28929         final double biasY = ba.getElementAtIndex(1);
28930 
28931         final Acceleration by2 = new Acceleration(biasY,
28932                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
28933         calibrator.setBiasY(by2);
28934 
28935         // check
28936         final Acceleration by3 = calibrator.getBiasYAsAcceleration();
28937         final Acceleration by4 = new Acceleration(biasY,
28938                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
28939         calibrator.getBiasYAsAcceleration(by4);
28940 
28941         assertEquals(by3, by4);
28942     }
28943 
28944     @Test
28945     public void testGetSetBiasZAsAcceleration() throws LockedException {
28946         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28947                 new KnownBiasAndGravityNormAccelerometerCalibrator();
28948 
28949         // check default value
28950         final Acceleration bz1 = calibrator.getBiasZAsAcceleration();
28951 
28952         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
28953         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28954 
28955         // set new value
28956         final Matrix ba = generateBa();
28957         final double biasZ = ba.getElementAtIndex(2);
28958 
28959         final Acceleration bz2 = new Acceleration(biasZ,
28960                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
28961         calibrator.setBiasZ(bz2);
28962 
28963         // check
28964         final Acceleration bz3 = calibrator.getBiasZAsAcceleration();
28965         final Acceleration bz4 = new Acceleration(biasZ,
28966                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
28967         calibrator.getBiasZAsAcceleration(bz4);
28968 
28969         assertEquals(bz3, bz4);
28970     }
28971 
28972     @Test
28973     public void testSetBias1() throws LockedException {
28974         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28975                 new KnownBiasAndGravityNormAccelerometerCalibrator();
28976 
28977         // check default values
28978         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
28979         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
28980         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
28981 
28982         // set new values
28983         final Matrix ba = generateBa();
28984         final double biasX = ba.getElementAtIndex(0);
28985         final double biasY = ba.getElementAtIndex(1);
28986         final double biasZ = ba.getElementAtIndex(2);
28987 
28988         calibrator.setBias(biasX, biasY, biasZ);
28989 
28990         // check
28991         assertEquals(calibrator.getBiasX(), biasX, 0.0);
28992         assertEquals(calibrator.getBiasY(), biasY, 0.0);
28993         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
28994     }
28995 
28996     @Test
28997     public void testSetBias2() throws LockedException {
28998         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
28999                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29000 
29001         // check default values
29002         assertEquals(calibrator.getBiasX(), 0.0, 0.0);
29003         assertEquals(calibrator.getBiasY(), 0.0, 0.0);
29004         assertEquals(calibrator.getBiasZ(), 0.0, 0.0);
29005 
29006         // set new values
29007         final Matrix ba = generateBa();
29008         final double biasX = ba.getElementAtIndex(0);
29009         final double biasY = ba.getElementAtIndex(1);
29010         final double biasZ = ba.getElementAtIndex(2);
29011 
29012         final Acceleration bx = new Acceleration(biasX,
29013                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29014         final Acceleration by = new Acceleration(biasY,
29015                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29016         final Acceleration bz = new Acceleration(biasZ,
29017                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29018 
29019         calibrator.setBias(bx, by, bz);
29020 
29021         // check
29022         assertEquals(calibrator.getBiasX(), biasX, 0.0);
29023         assertEquals(calibrator.getBiasY(), biasY, 0.0);
29024         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
29025     }
29026 
29027     @Test
29028     public void testGetSetInitialSx() throws WrongSizeException, LockedException {
29029         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29030                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29031 
29032         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29033 
29034         // set new value
29035         final Matrix ma = generateMaGeneral();
29036         final double sx = ma.getElementAt(0, 0);
29037 
29038         calibrator.setInitialSx(sx);
29039 
29040         // check
29041         assertEquals(calibrator.getInitialSx(), sx, 0.0);
29042     }
29043 
29044     @Test
29045     public void testGetSetInitialSy() throws WrongSizeException, LockedException {
29046         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29047                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29048 
29049         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29050 
29051         // set new value
29052         final Matrix ma = generateMaGeneral();
29053         final double sy = ma.getElementAt(1, 1);
29054 
29055         calibrator.setInitialSy(sy);
29056 
29057         // check
29058         assertEquals(calibrator.getInitialSy(), sy, 0.0);
29059     }
29060 
29061     @Test
29062     public void testGetSetInitialSz() throws WrongSizeException, LockedException {
29063         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29064                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29065 
29066         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29067 
29068         // set new value
29069         final Matrix ma = generateMaGeneral();
29070         final double sz = ma.getElementAt(2, 2);
29071 
29072         calibrator.setInitialSz(sz);
29073 
29074         // check
29075         assertEquals(calibrator.getInitialSz(), sz, 0.0);
29076     }
29077 
29078     @Test
29079     public void testGetSetInitialMxy() throws WrongSizeException, LockedException {
29080         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29081                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29082 
29083         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29084 
29085         // set new value
29086         final Matrix ma = generateMaGeneral();
29087         final double mxy = ma.getElementAt(0, 1);
29088 
29089         calibrator.setInitialMxy(mxy);
29090 
29091         // check
29092         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29093     }
29094 
29095     @Test
29096     public void testGetSetInitialMxz() throws WrongSizeException, LockedException {
29097         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29098                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29099 
29100         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29101 
29102         // set new value
29103         final Matrix ma = generateMaGeneral();
29104         final double mxz = ma.getElementAt(0, 2);
29105 
29106         calibrator.setInitialMxz(mxz);
29107 
29108         // check
29109         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29110     }
29111 
29112     @Test
29113     public void testGetSetInitialMyx() throws WrongSizeException, LockedException {
29114         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29115                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29116 
29117         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29118 
29119         // set new value
29120         final Matrix ma = generateMaGeneral();
29121         final double myx = ma.getElementAt(1, 0);
29122 
29123         calibrator.setInitialMyx(myx);
29124 
29125         // check
29126         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29127     }
29128 
29129     @Test
29130     public void testGetSetInitialMyz() throws WrongSizeException, LockedException {
29131         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29132                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29133 
29134         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29135 
29136         // set new value
29137         final Matrix ma = generateMaGeneral();
29138         final double myz = ma.getElementAt(1, 2);
29139 
29140         calibrator.setInitialMyz(myz);
29141 
29142         // check
29143         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29144     }
29145 
29146     @Test
29147     public void testGetSetInitialMzx() throws WrongSizeException, LockedException {
29148         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29149                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29150 
29151         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29152 
29153         // set new value
29154         final Matrix ma = generateMaGeneral();
29155         final double mzx = ma.getElementAt(2, 0);
29156 
29157         calibrator.setInitialMzx(mzx);
29158 
29159         // check
29160         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29161     }
29162 
29163     @Test
29164     public void testGetSetInitialMzy() throws WrongSizeException, LockedException {
29165         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29166                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29167 
29168         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29169 
29170         // set new value
29171         final Matrix ma = generateMaGeneral();
29172         final double mzy = ma.getElementAt(2, 1);
29173 
29174         calibrator.setInitialMzy(mzy);
29175 
29176         // check
29177         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29178     }
29179 
29180     @Test
29181     public void testSetInitialScalingFactors()
29182             throws WrongSizeException, LockedException {
29183 
29184         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29185                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29186 
29187         // check default values
29188         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29189         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29190         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29191 
29192         // set new values
29193         final Matrix ma = generateMaGeneral();
29194         final double sx = ma.getElementAt(0, 0);
29195         final double sy = ma.getElementAt(1, 1);
29196         final double sz = ma.getElementAt(2, 2);
29197 
29198         calibrator.setInitialScalingFactors(sx, sy, sz);
29199 
29200         // check
29201         assertEquals(calibrator.getInitialSx(), sx, 0.0);
29202         assertEquals(calibrator.getInitialSy(), sy, 0.0);
29203         assertEquals(calibrator.getInitialSz(), sz, 0.0);
29204     }
29205 
29206     @Test
29207     public void testSetInitialCrossCouplingErrors()
29208             throws WrongSizeException, LockedException {
29209 
29210         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29211                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29212 
29213         // check default values
29214         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29215         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29216         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29217         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29218         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29219         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29220 
29221         // set new values
29222         final Matrix ma = generateMaGeneral();
29223         final double mxy = ma.getElementAt(0, 1);
29224         final double mxz = ma.getElementAt(0, 2);
29225         final double myx = ma.getElementAt(1, 0);
29226         final double myz = ma.getElementAt(1, 2);
29227         final double mzx = ma.getElementAt(2, 0);
29228         final double mzy = ma.getElementAt(2, 1);
29229 
29230         calibrator.setInitialCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
29231 
29232         // check
29233         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29234         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29235         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29236         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29237         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29238         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29239     }
29240 
29241     @Test
29242     public void testSetInitialScalingFactorsAndCrossCouplingErrors()
29243             throws WrongSizeException, LockedException {
29244 
29245         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29246                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29247 
29248         // check default values
29249         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29250         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29251         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29252         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29253         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29254         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29255         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29256         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29257         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29258 
29259         // set new values
29260         final Matrix ma = generateMaGeneral();
29261         final double sx = ma.getElementAt(0, 0);
29262         final double sy = ma.getElementAt(1, 1);
29263         final double sz = ma.getElementAt(2, 2);
29264         final double mxy = ma.getElementAt(0, 1);
29265         final double mxz = ma.getElementAt(0, 2);
29266         final double myx = ma.getElementAt(1, 0);
29267         final double myz = ma.getElementAt(1, 2);
29268         final double mzx = ma.getElementAt(2, 0);
29269         final double mzy = ma.getElementAt(2, 1);
29270 
29271         calibrator.setInitialScalingFactorsAndCrossCouplingErrors(sx, sy, sz,
29272                 mxy, mxz, myx, myz, mzx, mzy);
29273 
29274         // check
29275         assertEquals(calibrator.getInitialSx(), sx, 0.0);
29276         assertEquals(calibrator.getInitialSy(), sy, 0.0);
29277         assertEquals(calibrator.getInitialSz(), sz, 0.0);
29278         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29279         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29280         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29281         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29282         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29283         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29284     }
29285 
29286     @Test
29287     public void testGetSetBias() throws LockedException {
29288         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29289                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29290 
29291         // check default value
29292         final double[] bias1 = calibrator.getBias();
29293         assertArrayEquals(bias1, new double[3], 0.0);
29294 
29295         // set new values
29296         final Matrix ba = generateBa();
29297         final double[] bias2 = ba.getBuffer();
29298         calibrator.setBias(bias2);
29299 
29300         // check
29301         final double[] bias3 = calibrator.getBias();
29302         final double[] bias4 = new double[3];
29303         calibrator.getBias(bias4);
29304 
29305         assertArrayEquals(bias2, bias3, 0.0);
29306         assertArrayEquals(bias2, bias4, 0.0);
29307 
29308         // Force IllegalArgumentException
29309         try {
29310             calibrator.getBias(new double[1]);
29311             fail("IllegalArgumentException expected but not thrown");
29312         } catch (final IllegalArgumentException ignore) {
29313         }
29314         try {
29315             calibrator.setBias(new double[1]);
29316             fail("IllegalArgumentException expected but not thrown");
29317         } catch (final IllegalArgumentException ignore) {
29318         }
29319     }
29320 
29321     @Test
29322     public void testGetSetBiasAsMatrix() throws WrongSizeException,
29323             LockedException {
29324 
29325         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29326                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29327 
29328         // check default value
29329         final Matrix bias1 = calibrator.getBiasAsMatrix();
29330         assertEquals(bias1, new Matrix(3, 1));
29331 
29332         // set new value
29333         final Matrix bias2 = generateBa();
29334         calibrator.setBias(bias2);
29335 
29336         // check
29337         final Matrix bias3 = calibrator.getBiasAsMatrix();
29338         final Matrix bias4 = new Matrix(3, 1);
29339         calibrator.getBiasAsMatrix(bias4);
29340 
29341         assertEquals(bias2, bias3);
29342         assertEquals(bias2, bias4);
29343 
29344         // Force IllegalArgumentException
29345         try {
29346             calibrator.getBiasAsMatrix(new Matrix(1, 1));
29347             fail("IllegalArgumentException expected but not thrown");
29348         } catch (final IllegalArgumentException ignore) {
29349         }
29350         try {
29351             calibrator.getBiasAsMatrix(new Matrix(3, 3));
29352             fail("IllegalArgumentException expected but not thrown");
29353         } catch (final IllegalArgumentException ignore) {
29354         }
29355         try {
29356             calibrator.setBias(new Matrix(1, 1));
29357             fail("IllegalArgumentException expected but not thrown");
29358         } catch (final IllegalArgumentException ignore) {
29359         }
29360         try {
29361             calibrator.setBias(new Matrix(3, 3));
29362             fail("IllegalArgumentException expected but not thrown");
29363         } catch (final IllegalArgumentException ignore) {
29364         }
29365     }
29366 
29367     @Test
29368     public void testGetSetInitialMa() throws WrongSizeException, LockedException {
29369         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29370                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29371 
29372         // test default value
29373         final Matrix ma1 = calibrator.getInitialMa();
29374         assertEquals(ma1, new Matrix(3, 3));
29375 
29376         // set new value
29377         final Matrix ma2 = generateMaGeneral();
29378         calibrator.setInitialMa(ma2);
29379 
29380         // check
29381         final Matrix ma3 = calibrator.getInitialMa();
29382         final Matrix ma4 = new Matrix(3, 3);
29383         calibrator.getInitialMa(ma4);
29384 
29385         assertEquals(ma2, ma3);
29386         assertEquals(ma2, ma4);
29387 
29388         // Force IllegalArgumentException
29389         try {
29390             calibrator.getInitialMa(new Matrix(1, 3));
29391             fail("IllegalArgumentException expected but not thrown");
29392         } catch (final IllegalArgumentException ignore) {
29393         }
29394         try {
29395             calibrator.getInitialMa(new Matrix(3, 1));
29396             fail("IllegalArgumentException expected but not thrown");
29397         } catch (final IllegalArgumentException ignore) {
29398         }
29399         try {
29400             calibrator.setInitialMa(new Matrix(1, 3));
29401             fail("IllegalArgumentException expected but not thrown");
29402         } catch (final IllegalArgumentException ignore) {
29403         }
29404         try {
29405             calibrator.setInitialMa(new Matrix(3, 1));
29406             fail("IllegalArgumentException expected but not thrown");
29407         } catch (final IllegalArgumentException ignore) {
29408         }
29409     }
29410 
29411     @Test
29412     public void testGetSetMeasurements() throws LockedException {
29413         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29414                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29415 
29416         // check default value
29417         assertNull(calibrator.getMeasurements());
29418 
29419         // set new value
29420         final Collection<StandardDeviationBodyKinematics> measurements =
29421                 Collections.emptyList();
29422         calibrator.setMeasurements(measurements);
29423 
29424         // check
29425         assertSame(calibrator.getMeasurements(), measurements);
29426     }
29427 
29428     @Test
29429     public void testIsSetCommonAxisUsed() throws LockedException {
29430         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29431                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29432 
29433         // check default value
29434         assertFalse(calibrator.isCommonAxisUsed());
29435 
29436         // set new value
29437         calibrator.setCommonAxisUsed(true);
29438 
29439         // check
29440         assertTrue(calibrator.isCommonAxisUsed());
29441     }
29442 
29443     @Test
29444     public void testGetSetListener() throws LockedException {
29445         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29446                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29447 
29448         // check default value
29449         assertNull(calibrator.getListener());
29450 
29451         // set new value
29452         calibrator.setListener(this);
29453 
29454         // check
29455         assertSame(calibrator.getListener(), this);
29456     }
29457 
29458     @Test
29459     public void testGetSetGroundTruthGravityNorm1() throws LockedException {
29460         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29461                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29462 
29463         // check default value
29464         assertNull(calibrator.getGroundTruthGravityNorm());
29465 
29466         // set new value
29467         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29468         final double latitude = Math.toRadians(
29469                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29470         final double longitude = Math.toRadians(
29471                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29472         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29473         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29474         final NEDVelocity nedVelocity = new NEDVelocity();
29475         final ECEFPosition ecefPosition = new ECEFPosition();
29476         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29477         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29478                 ecefPosition, ecefVelocity);
29479         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29480                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29481         final double gravityNorm = gravity.getNorm();
29482 
29483         calibrator.setGroundTruthGravityNorm(gravityNorm);
29484 
29485         // check
29486         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(),
29487                 0.0);
29488         assertEquals(gravity.getNormAsAcceleration(),
29489                 calibrator.getGroundTruthGravityNormAsAcceleration());
29490         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29491         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29492         assertEquals(gravity.getNormAsAcceleration(), g);
29493     }
29494 
29495     @Test
29496     public void testGetSetGroundTruthGravityNorm2() throws LockedException {
29497         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29498                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29499 
29500         // check default value
29501         assertNull(calibrator.getGroundTruthGravityNorm());
29502 
29503         // set new value
29504         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29505         final double latitude = Math.toRadians(
29506                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29507         final double longitude = Math.toRadians(
29508                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29509         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29510         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29511         final NEDVelocity nedVelocity = new NEDVelocity();
29512         final ECEFPosition ecefPosition = new ECEFPosition();
29513         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29514         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29515                 ecefPosition, ecefVelocity);
29516         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29517                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29518         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
29519 
29520         calibrator.setGroundTruthGravityNorm(gravityNorm);
29521 
29522         // check
29523         assertEquals(gravity.getNorm(),
29524                 calibrator.getGroundTruthGravityNorm(), 0.0);
29525         assertEquals(gravityNorm,
29526                 calibrator.getGroundTruthGravityNormAsAcceleration());
29527         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29528         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29529         assertEquals(gravityNorm, g);
29530     }
29531 
29532     @Test
29533     public void testIsReady() throws LockedException {
29534         final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29535                 new KnownBiasAndGravityNormAccelerometerCalibrator();
29536 
29537         // check
29538         assertFalse(calibrator.isReady());
29539 
29540 
29541         // set empty measurements
29542         final Collection<StandardDeviationBodyKinematics> measurements1 =
29543                 Collections.emptyList();
29544         calibrator.setMeasurements(measurements1);
29545 
29546         // check
29547         assertFalse(calibrator.isReady());
29548 
29549 
29550         // set enough measurements for general case
29551         calibrator.setCommonAxisUsed(false);
29552 
29553         final List<StandardDeviationBodyKinematics> measurements2 = new ArrayList<>();
29554         for (int i = 0; i < KnownBiasAndGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
29555             measurements2.add(new StandardDeviationBodyKinematics());
29556         }
29557         calibrator.setMeasurements(measurements2);
29558 
29559         // check
29560         assertFalse(calibrator.isReady());
29561 
29562 
29563         // set gravity norm
29564         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29565         final double latitude = Math.toRadians(
29566                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29567         final double longitude = Math.toRadians(
29568                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29569         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29570         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29571         final NEDVelocity nedVelocity = new NEDVelocity();
29572         final ECEFPosition ecefPosition = new ECEFPosition();
29573         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29574         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29575                 ecefPosition, ecefVelocity);
29576         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29577                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29578 
29579         calibrator.setGroundTruthGravityNorm(gravity.getNorm());
29580 
29581         assertTrue(calibrator.isReady());
29582 
29583         // set enough measurements for common axis case
29584         measurements2.clear();
29585         for (int i = 0; i < KnownBiasAndGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
29586             measurements2.add(new StandardDeviationBodyKinematics());
29587         }
29588         calibrator.setMeasurements(measurements2);
29589 
29590         // check
29591         assertFalse(calibrator.isReady());
29592 
29593         calibrator.setCommonAxisUsed(true);
29594 
29595         assertTrue(calibrator.isReady());
29596     }
29597 
29598     @Test
29599     public void testCalibrateForGeneralCaseWithMinimumMeasuresAndNoNoise()
29600             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
29601             LockedException, NotReadyException {
29602 
29603         int numValid = 0;
29604         for (int t = 0; t < TIMES; t++) {
29605             final Matrix ba = generateBa();
29606             final Matrix bg = generateBg();
29607             final Matrix ma = generateMaGeneral();
29608             final Matrix mg = generateMg();
29609             final Matrix gg = generateGg();
29610             // when using minimum number of measurements we must not add any noise so that
29611             // a solution is found, when adding more measurements, certain noise can be added
29612             final double accelNoiseRootPSD = 0.0;
29613             final double gyroNoiseRootPSD = 0.0;
29614             final double accelQuantLevel = 0.0;
29615             final double gyroQuantLevel = 0.0;
29616 
29617             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
29618                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
29619 
29620             final Random random = new Random();
29621             final UniformRandomizer randomizer = new UniformRandomizer(random);
29622             final double latitude = Math.toRadians(
29623                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29624             final double longitude = Math.toRadians(
29625                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29626             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29627             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29628             final NEDVelocity nedVelocity = new NEDVelocity();
29629             final ECEFPosition ecefPosition = new ECEFPosition();
29630             final ECEFVelocity ecefVelocity = new ECEFVelocity();
29631             NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29632                     ecefPosition, ecefVelocity);
29633             final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29634                     ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29635 
29636             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
29637             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
29638             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
29639 
29640             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
29641             for (int i = 0; i < KnownBiasAndGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
29642                 final double roll = Math.toRadians(
29643                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29644                 final double pitch = Math.toRadians(
29645                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29646                 final double yaw = Math.toRadians(
29647                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29648                 final CoordinateTransformation nedC = new CoordinateTransformation(
29649                         roll, pitch, yaw, FrameType.BODY_FRAME,
29650                         FrameType.LOCAL_NAVIGATION_FRAME);
29651 
29652                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
29653                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
29654                         .convertNEDtoECEFAndReturnNew(nedFrame);
29655 
29656                 // compute ground-truth kinematics that should be generated at provided
29657                 // position, velocity and orientation
29658                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
29659                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
29660                                 ecefFrame);
29661 
29662                 // apply known calibration parameters to distort ground-truth and generate a
29663                 // measured kinematics sample
29664                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
29665                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
29666 
29667                 final StandardDeviationBodyKinematics measurement =
29668                         new StandardDeviationBodyKinematics(measuredKinematics,
29669                                 specificForceStandardDeviation,
29670                                 angularRateStandardDeviation);
29671                 measurements.add(measurement);
29672             }
29673 
29674             // When we have the minimum number of measurements, we need to provide
29675             // an initial solution close to the true solution
29676             final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29677                     new KnownBiasAndGravityNormAccelerometerCalibrator(
29678                             gravity.getNorm(), measurements,
29679                             false, ba, ma, this);
29680 
29681             // estimate
29682             reset();
29683             assertTrue(calibrator.isReady());
29684             assertFalse(calibrator.isRunning());
29685             assertEquals(mCalibrateStart, 0);
29686             assertEquals(mCalibrateEnd, 0);
29687 
29688             try {
29689                 calibrator.calibrate();
29690             } catch (final CalibrationException e) {
29691                 continue;
29692             }
29693 
29694             // check
29695             assertTrue(calibrator.isReady());
29696             assertFalse(calibrator.isRunning());
29697             assertEquals(mCalibrateStart, 1);
29698             assertEquals(mCalibrateEnd, 1);
29699 
29700             final Matrix estimatedMa = calibrator.getEstimatedMa();
29701 
29702             if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
29703                 continue;
29704             }
29705 
29706             assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
29707 
29708             numValid++;
29709 
29710             break;
29711         }
29712 
29713         assertTrue(numValid > 0);
29714     }
29715 
29716     @Test
29717     public void testCalibrateForGeneralCaseWithLargeNumberOfMeasurementsAndNoise()
29718             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
29719             LockedException, NotReadyException {
29720 
29721         int numValid = 0;
29722         for (int t = 0; t < TIMES; t++) {
29723             final Matrix ba = generateBa();
29724             final Matrix bg = generateBg();
29725             final Matrix ma = generateMaGeneral();
29726             final Matrix mg = generateMg();
29727             final Matrix gg = generateGg();
29728             final double accelNoiseRootPSD = getAccelNoiseRootPSD();
29729             final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
29730             final double accelQuantLevel = 0.0;
29731             final double gyroQuantLevel = 0.0;
29732 
29733             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
29734                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
29735 
29736             final Random random = new Random();
29737             final UniformRandomizer randomizer = new UniformRandomizer(random);
29738             final double latitude = Math.toRadians(
29739                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29740             final double longitude = Math.toRadians(
29741                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29742             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29743             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29744             final NEDVelocity nedVelocity = new NEDVelocity();
29745             final ECEFPosition ecefPosition = new ECEFPosition();
29746             final ECEFVelocity ecefVelocity = new ECEFVelocity();
29747             NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29748                     ecefPosition, ecefVelocity);
29749             final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29750                     ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29751 
29752             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
29753             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
29754             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
29755 
29756             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
29757             for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
29758                 final double roll = Math.toRadians(
29759                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29760                 final double pitch = Math.toRadians(
29761                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29762                 final double yaw = Math.toRadians(
29763                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29764                 final CoordinateTransformation nedC = new CoordinateTransformation(
29765                         roll, pitch, yaw, FrameType.BODY_FRAME,
29766                         FrameType.LOCAL_NAVIGATION_FRAME);
29767 
29768                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
29769                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
29770                         .convertNEDtoECEFAndReturnNew(nedFrame);
29771 
29772                 // compute ground-truth kinematics that should be generated at provided
29773                 // position, velocity and orientation
29774                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
29775                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
29776                                 ecefFrame);
29777 
29778                 // apply known calibration parameters to distort ground-truth and generate a
29779                 // measured kinematics sample
29780                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
29781                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
29782 
29783                 final StandardDeviationBodyKinematics measurement =
29784                         new StandardDeviationBodyKinematics(measuredKinematics,
29785                                 specificForceStandardDeviation,
29786                                 angularRateStandardDeviation);
29787                 measurements.add(measurement);
29788             }
29789 
29790             // When we have a large number of measurements, we do not need to provide
29791             // an initial solution as it will probably converge to true true solution
29792             final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29793                     new KnownBiasAndGravityNormAccelerometerCalibrator(
29794                             gravity.getNorm(),
29795                             measurements, false, this);
29796 
29797             // estimate
29798             reset();
29799             assertTrue(calibrator.isReady());
29800             assertFalse(calibrator.isRunning());
29801             assertEquals(mCalibrateStart, 0);
29802             assertEquals(mCalibrateEnd, 0);
29803 
29804             try {
29805                 calibrator.calibrate();
29806             } catch (final CalibrationException e) {
29807                 continue;
29808             }
29809 
29810             // check
29811             assertTrue(calibrator.isReady());
29812             assertFalse(calibrator.isRunning());
29813             assertEquals(mCalibrateStart, 1);
29814             assertEquals(mCalibrateEnd, 1);
29815 
29816             final Matrix estimatedMa = calibrator.getEstimatedMa();
29817 
29818             if (!ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR)) {
29819                 continue;
29820             }
29821 
29822             assertTrue(ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR));
29823 
29824             numValid++;
29825 
29826             break;
29827         }
29828 
29829         assertTrue(numValid > 0);
29830     }
29831 
29832     @Test
29833     public void testCalibrateForCommonAxisCaseWithMinimumMeasuresAndNoNoise()
29834             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
29835             LockedException, NotReadyException {
29836 
29837         int numValid = 0;
29838         for (int t = 0; t < TIMES; t++) {
29839             final Matrix ba = generateBa();
29840             final Matrix bg = generateBg();
29841             final Matrix ma = generateMaCommonAxis();
29842             final Matrix mg = generateMg();
29843             final Matrix gg = generateGg();
29844             // when using minimum number of measurements we must not add any noise so that
29845             // a solution is found, when adding more measurements, certain noise can be added
29846             final double accelNoiseRootPSD = 0.0;
29847             final double gyroNoiseRootPSD = 0.0;
29848             final double accelQuantLevel = 0.0;
29849             final double gyroQuantLevel = 0.0;
29850 
29851             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
29852                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
29853 
29854             final Random random = new Random();
29855             final UniformRandomizer randomizer = new UniformRandomizer(random);
29856             final double latitude = Math.toRadians(
29857                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29858             final double longitude = Math.toRadians(
29859                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29860             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29861             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29862             final NEDVelocity nedVelocity = new NEDVelocity();
29863             final ECEFPosition ecefPosition = new ECEFPosition();
29864             final ECEFVelocity ecefVelocity = new ECEFVelocity();
29865             NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29866                     ecefPosition, ecefVelocity);
29867             final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29868                     ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29869 
29870             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
29871             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
29872             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
29873 
29874             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
29875             for (int i = 0; i < KnownBiasAndGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
29876                 final double roll = Math.toRadians(
29877                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29878                 final double pitch = Math.toRadians(
29879                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29880                 final double yaw = Math.toRadians(
29881                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29882                 final CoordinateTransformation nedC = new CoordinateTransformation(
29883                         roll, pitch, yaw, FrameType.BODY_FRAME,
29884                         FrameType.LOCAL_NAVIGATION_FRAME);
29885 
29886                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
29887                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
29888                         .convertNEDtoECEFAndReturnNew(nedFrame);
29889 
29890                 // compute ground-truth kinematics that should be generated at provided
29891                 // position, velocity and orientation
29892                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
29893                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
29894                                 ecefFrame);
29895 
29896                 // apply known calibration parameters to distort ground-truth and generate a
29897                 // measured kinematics sample
29898                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
29899                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
29900 
29901                 final StandardDeviationBodyKinematics measurement =
29902                         new StandardDeviationBodyKinematics(measuredKinematics,
29903                                 specificForceStandardDeviation,
29904                                 angularRateStandardDeviation);
29905                 measurements.add(measurement);
29906             }
29907 
29908             // When we have the minimum number of measurements, we need to provide
29909             // an initial solution close to the true solution
29910             final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
29911                     new KnownBiasAndGravityNormAccelerometerCalibrator(
29912                             gravity.getNorm(), measurements,
29913                             true, ba, ma, this);
29914 
29915             // estimate
29916             reset();
29917             assertTrue(calibrator.isReady());
29918             assertFalse(calibrator.isRunning());
29919             assertEquals(mCalibrateStart, 0);
29920             assertEquals(mCalibrateEnd, 0);
29921 
29922             try {
29923                 calibrator.calibrate();
29924             } catch (final CalibrationException e) {
29925                 continue;
29926             }
29927 
29928             // check
29929             assertTrue(calibrator.isReady());
29930             assertFalse(calibrator.isRunning());
29931             assertEquals(mCalibrateStart, 1);
29932             assertEquals(mCalibrateEnd, 1);
29933 
29934             final Matrix estimatedMa = calibrator.getEstimatedMa();
29935 
29936             if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
29937                 continue;
29938             }
29939 
29940             assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
29941 
29942             numValid++;
29943 
29944             break;
29945         }
29946 
29947         assertTrue(numValid > 0);
29948     }
29949 
29950     @Test
29951     public void testCalibrateForCommonAxisCaseWithLargeNumberOfMeasurementsAndNoise()
29952             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
29953             LockedException, NotReadyException {
29954 
29955         int numValid = 0;
29956         for (int t = 0; t < TIMES; t++) {
29957             final Matrix ba = generateBa();
29958             final Matrix bg = generateBg();
29959             final Matrix ma = generateMaCommonAxis();
29960             final Matrix mg = generateMg();
29961             final Matrix gg = generateGg();
29962             final double accelNoiseRootPSD = getAccelNoiseRootPSD();
29963             final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
29964             final double accelQuantLevel = 0.0;
29965             final double gyroQuantLevel = 0.0;
29966 
29967             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
29968                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
29969 
29970             final Random random = new Random();
29971             final UniformRandomizer randomizer = new UniformRandomizer(random);
29972             final double latitude = Math.toRadians(
29973                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29974             final double longitude = Math.toRadians(
29975                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29976             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29977             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29978             final NEDVelocity nedVelocity = new NEDVelocity();
29979             final ECEFPosition ecefPosition = new ECEFPosition();
29980             final ECEFVelocity ecefVelocity = new ECEFVelocity();
29981             NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29982                     ecefPosition, ecefVelocity);
29983             final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29984                     ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29985 
29986             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
29987             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
29988             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
29989 
29990             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
29991             for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
29992                 final double roll = Math.toRadians(
29993                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29994                 final double pitch = Math.toRadians(
29995                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29996                 final double yaw = Math.toRadians(
29997                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29998                 final CoordinateTransformation nedC = new CoordinateTransformation(
29999                         roll, pitch, yaw, FrameType.BODY_FRAME,
30000                         FrameType.LOCAL_NAVIGATION_FRAME);
30001 
30002                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
30003                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
30004                         .convertNEDtoECEFAndReturnNew(nedFrame);
30005 
30006                 // compute ground-truth kinematics that should be generated at provided
30007                 // position, velocity and orientation
30008                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
30009                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
30010                                 ecefFrame);
30011 
30012                 // apply known calibration parameters to distort ground-truth and generate a
30013                 // measured kinematics sample
30014                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
30015                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
30016 
30017                 final StandardDeviationBodyKinematics measurement =
30018                         new StandardDeviationBodyKinematics(measuredKinematics,
30019                                 specificForceStandardDeviation,
30020                                 angularRateStandardDeviation);
30021                 measurements.add(measurement);
30022             }
30023 
30024             // When we have a large number of measurements, we do not need to provide
30025             // an initial solution as it will probably converge to true true solution
30026             final KnownBiasAndGravityNormAccelerometerCalibrator calibrator =
30027                     new KnownBiasAndGravityNormAccelerometerCalibrator(
30028                             gravity.getNorm(), measurements,
30029                             true, this);
30030 
30031             // estimate
30032             reset();
30033             assertTrue(calibrator.isReady());
30034             assertFalse(calibrator.isRunning());
30035             assertEquals(mCalibrateStart, 0);
30036             assertEquals(mCalibrateEnd, 0);
30037 
30038             try {
30039                 calibrator.calibrate();
30040             } catch (final CalibrationException e) {
30041                 continue;
30042             }
30043 
30044             // check
30045             assertTrue(calibrator.isReady());
30046             assertFalse(calibrator.isRunning());
30047             assertEquals(mCalibrateStart, 1);
30048             assertEquals(mCalibrateEnd, 1);
30049 
30050             final Matrix estimatedMa = calibrator.getEstimatedMa();
30051 
30052             if (!ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR)) {
30053                 continue;
30054             }
30055 
30056             assertTrue(ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR));
30057 
30058             numValid++;
30059 
30060             break;
30061         }
30062 
30063         assertTrue(numValid > 0);
30064     }
30065 
30066     @Override
30067     public void onCalibrateStart(final KnownBiasAndGravityNormAccelerometerCalibrator calibrator) {
30068         checkLocked(calibrator);
30069         mCalibrateStart++;
30070     }
30071 
30072     @Override
30073     public void onCalibrateEnd(final KnownBiasAndGravityNormAccelerometerCalibrator calibrator) {
30074         checkLocked(calibrator);
30075         mCalibrateEnd++;
30076     }
30077 
30078     private void reset() {
30079         mCalibrateStart = 0;
30080         mCalibrateEnd = 0;
30081     }
30082 
30083     private void checkLocked(final KnownBiasAndGravityNormAccelerometerCalibrator calibrator) {
30084         assertTrue(calibrator.isRunning());
30085         try {
30086             calibrator.setBiasX(0.0);
30087             fail("LockedException expected but not thrown");
30088         } catch (final LockedException ignore) {
30089         }
30090         try {
30091             calibrator.setBiasY(0.0);
30092             fail("LockedException expected but not thrown");
30093         } catch (final LockedException ignore) {
30094         }
30095         try {
30096             calibrator.setBiasZ(0.0);
30097             fail("LockedException expected but not thrown");
30098         } catch (final LockedException ignore) {
30099         }
30100         try {
30101             calibrator.setBiasX(null);
30102             fail("LockedException expected but not thrown");
30103         } catch (final LockedException ignore) {
30104         }
30105         try {
30106             calibrator.setBiasY(null);
30107             fail("LockedException expected but not thrown");
30108         } catch (final LockedException ignore) {
30109         }
30110         try {
30111             calibrator.setBiasZ(null);
30112             fail("LockedException expected but not thrown");
30113         } catch (final LockedException ignore) {
30114         }
30115         try {
30116             calibrator.setBias(0.0, 0.0, 0.0);
30117             fail("LockedException expected but not thrown");
30118         } catch (final LockedException ignore) {
30119         }
30120         try {
30121             calibrator.setBias(null, null, null);
30122             fail("LockedException expected but not thrown");
30123         } catch (final LockedException ignore) {
30124         }
30125         try {
30126             calibrator.setInitialSx(0.0);
30127             fail("LockedException expected but not thrown");
30128         } catch (final LockedException ignore) {
30129         }
30130         try {
30131             calibrator.setInitialSy(0.0);
30132             fail("LockedException expected but not thrown");
30133         } catch (final LockedException ignore) {
30134         }
30135         try {
30136             calibrator.setInitialSz(0.0);
30137             fail("LockedException expected but not thrown");
30138         } catch (final LockedException ignore) {
30139         }
30140         try {
30141             calibrator.setInitialMxy(0.0);
30142             fail("LockedException expected but not thrown");
30143         } catch (final LockedException ignore) {
30144         }
30145         try {
30146             calibrator.setInitialMxz(0.0);
30147             fail("LockedException expected but not thrown");
30148         } catch (final LockedException ignore) {
30149         }
30150         try {
30151             calibrator.setInitialMyx(0.0);
30152             fail("LockedException expected but not thrown");
30153         } catch (final LockedException ignore) {
30154         }
30155         try {
30156             calibrator.setInitialMyz(0.0);
30157             fail("LockedException expected but not thrown");
30158         } catch (final LockedException ignore) {
30159         }
30160         try {
30161             calibrator.setInitialMzx(0.0);
30162             fail("LockedException expected but not thrown");
30163         } catch (final LockedException ignore) {
30164         }
30165         try {
30166             calibrator.setInitialMzy(0.0);
30167             fail("LockedException expected but not thrown");
30168         } catch (final LockedException ignore) {
30169         }
30170         try {
30171             calibrator.setInitialScalingFactors(0.0, 0.0, 0.0);
30172             fail("LockedException expected but not thrown");
30173         } catch (final LockedException ignore) {
30174         }
30175         try {
30176             calibrator.setInitialCrossCouplingErrors(
30177                     0.0, 0.0, 0.0,
30178                     0.0, 0.0, 0.0);
30179             fail("LockedException expected but not thrown");
30180         } catch (final LockedException ignore) {
30181         }
30182         try {
30183             calibrator.setInitialScalingFactorsAndCrossCouplingErrors(
30184                     0.0, 0.0, 0.0,
30185                     0.0, 0.0, 0.0,
30186                     0.0, 0.0, 0.0);
30187             fail("LockedException expected but not thrown");
30188         } catch (final LockedException ignore) {
30189         }
30190         try {
30191             calibrator.setBias((double[]) null);
30192             fail("LockedException expected but not thrown");
30193         } catch (final LockedException ignore) {
30194         }
30195         try {
30196             calibrator.setBias((Matrix) null);
30197             fail("LockedException expected but not thrown");
30198         } catch (final LockedException ignore) {
30199         }
30200         try {
30201             calibrator.setInitialMa(null);
30202             fail("LockedException expected but not thrown");
30203         } catch (final LockedException ignore) {
30204         }
30205         try {
30206             calibrator.setGroundTruthGravityNorm(0.0);
30207             fail("LockedException expected but not thrown");
30208         } catch (final LockedException ignore) {
30209         }
30210         try {
30211             calibrator.setGroundTruthGravityNorm((Acceleration) null);
30212             fail("LockedException expected but not thrown");
30213         } catch (final LockedException ignore) {
30214         }
30215         try {
30216             calibrator.setMeasurements(null);
30217             fail("LockedException expected but not thrown");
30218         } catch (final LockedException ignore) {
30219         }
30220         try {
30221             calibrator.setCommonAxisUsed(true);
30222             fail("LockedException expected but not thrown");
30223         } catch (final LockedException ignore) {
30224         }
30225         try {
30226             calibrator.setListener(this);
30227             fail("LockedException expected but not thrown");
30228         } catch (final LockedException ignore) {
30229         }
30230         try {
30231             calibrator.calibrate();
30232             fail("LockedException expected but not thrown");
30233         } catch (final LockedException ignore) {
30234         } catch (final Exception e) {
30235             fail("LockedException expected but not thrown");
30236         }
30237     }
30238 
30239     private Matrix generateBa() {
30240         return Matrix.newFromArray(new double[]{
30241                 900 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
30242                 -1000 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
30243                 800 * MICRO_G_TO_METERS_PER_SECOND_SQUARED});
30244     }
30245 
30246     private Matrix generateBg() {
30247         return Matrix.newFromArray(new double[]{
30248                 -9 * DEG_TO_RAD / 3600.0,
30249                 10 * DEG_TO_RAD / 3600.0,
30250                 -8 * DEG_TO_RAD / 3600.0});
30251     }
30252 
30253     private Matrix generateMaGeneral() throws WrongSizeException {
30254         final Matrix result = new Matrix(3, 3);
30255         result.fromArray(new double[]{
30256                 500e-6, -300e-6, 200e-6,
30257                 -150e-6, -600e-6, 250e-6,
30258                 -250e-6, 70e-6, 450e-6
30259         }, false);
30260 
30261         return result;
30262     }
30263 
30264     private Matrix generateMaCommonAxis() throws WrongSizeException {
30265         final Matrix result = new Matrix(3, 3);
30266         result.fromArray(new double[]{
30267                 500e-6, -300e-6, 200e-6,
30268                 0.0, -600e-6, 250e-6,
30269                 0.0, 0.0, 450e-6
30270         }, false);
30271 
30272         return result;
30273     }
30274 
30275     private Matrix generateMg() throws WrongSizeException {
30276         final Matrix result = new Matrix(3, 3);
30277         result.fromArray(new double[]{
30278                 400e-6, -300e-6, 250e-6,
30279                 0.0, -300e-6, -150e-6,
30280                 0.0, 0.0, -350e-6
30281         }, false);
30282 
30283         return result;
30284     }
30285 
30286     private Matrix generateGg() throws WrongSizeException {
30287         final Matrix result = new Matrix(3, 3);
30288         final double tmp = DEG_TO_RAD / (3600 * 9.80665);
30289         result.fromArray(new double[]{
30290                 0.9 * tmp, -1.1 * tmp, -0.6 * tmp,
30291                 -0.5 * tmp, 1.9 * tmp, -1.6 * tmp,
30292                 0.3 * tmp, 1.1 * tmp, -1.3 * tmp
30293         }, false);
30294 
30295         return result;
30296     }
30297 
30298     private double getAccelNoiseRootPSD() {
30299         return 70.0 * MICRO_G_TO_METERS_PER_SECOND_SQUARED;
30300     }
30301 
30302     private double getGyroNoiseRootPSD() {
30303         return 0.01 * DEG_TO_RAD / 60.0;
30304     }
30305 }